From 7ca29bfaa7a23d06374c2456e0360c911bd9aa3e Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Sat, 22 Feb 2014 20:28:19 -0300 Subject: -added kinematic body -added kinematic body demos --- servers/physics_2d/collision_solver_2d_sat.cpp | 622 +++++++++++++++++-------- 1 file changed, 431 insertions(+), 191 deletions(-) (limited to 'servers/physics_2d/collision_solver_2d_sat.cpp') diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp index fdbbebefcf..7d85183645 100644 --- a/servers/physics_2d/collision_solver_2d_sat.cpp +++ b/servers/physics_2d/collision_solver_2d_sat.cpp @@ -321,7 +321,7 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po -template +template class SeparatorAxisTest2D { const ShapeA *shape_A; @@ -334,6 +334,8 @@ class SeparatorAxisTest2D { int best_axis_index; Vector2 motion_A; Vector2 motion_B; + real_t margin_A; + real_t margin_B; _CollectorCallback2D *callback; public: @@ -397,6 +399,13 @@ public: else 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_B -= ( max_A - min_A ) * 0.5; max_B += ( max_A - min_A ) * 0.5; @@ -468,6 +477,14 @@ public: } } + if (withMargin) { + + for(int i=0;ixform(supports_B[i]); } } + + if (withMargin) { + + for(int i=0;i -static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const SegmentShape2DSW *segment_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -570,24 +599,39 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_tr 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))) return; + 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())) ) + return; + 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())) ) + return; + if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_b())) ) + return; + } + separator.generate_contacts(); } -template -static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const CircleShape2DSW *circle_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -612,13 +656,13 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_tra separator.generate_contacts(); } -template -static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -635,17 +679,55 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_ if (!separator.test_axis(p_transform_b.elements[1].normalized())) return; + if (withMargin) { + + Matrix32 inv = p_transform_b.affine_inverse(); + + 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))) + return; + 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))) + return; + 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))) + return; + 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))) + return; + 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 -static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -671,13 +753,13 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_tr separator.generate_contacts(); } -template -static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const SegmentShape2DSW *segment_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -692,6 +774,16 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3 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) ))) + return; + if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) ))) + return; + + } + } separator.generate_contacts(); @@ -701,14 +793,14 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3 ///////// -template -static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const CircleShape2DSW *circle_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -724,14 +816,14 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_tran } -template -static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -741,7 +833,7 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t 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(); +// const Vector2& half_extents = rectangle_B->get_half_extents(); if (!separator.test_axis(axis[0].normalized())) return; @@ -749,75 +841,45 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t if (!separator.test_axis(axis[1].normalized())) return; + Matrix32 binv = p_transform_b.affine_inverse(); { - Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin()); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) ) return; } if (castA) { Vector2 sphereofs = sphere + p_motion_a; - Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) return; } if (castB) { Vector2 sphereofs = sphere - p_motion_b; - Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + 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; - Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - - if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized())) + if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) ) return; } separator.generate_contacts(); } -template -static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -840,14 +902,14 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_tra } -template -static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CircleShape2DSW *circle_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -872,14 +934,14 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32 ///////// -template -static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const RectangleShape2DSW *rectangle_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -901,17 +963,56 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& if (!separator.test_axis(p_transform_b.elements[1].normalized())) return; + if (withMargin) { + + Matrix32 invA=p_transform_a.affine_inverse(); + Matrix32 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) ) ) + return; + + if (castA || castB) { + + Matrix32 aofs = p_transform_a; + aofs.elements[2]+=p_motion_a; + + Matrix32 bofs = p_transform_b; + bofs.elements[2]+=p_motion_b; + + Matrix32 aofsinv = aofs.affine_inverse(); + Matrix32 bofsinv = bofs.affine_inverse(); + + if (castA) { + + 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) ) ) + return; + } + + if (castA && castB) { + + if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,bofs,bofsinv) ) ) + return; + } + } + } + separator.generate_contacts(); } -template -static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -940,15 +1041,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ { Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5); - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -957,16 +1050,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ 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; - - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -974,16 +1058,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ 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; - - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -993,15 +1068,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ capsule_endpoint+=p_motion_b; - const Vector2& half_extents = rectangle_A->get_half_extents(); - Vector2 local_v = boxinv.xform(capsule_endpoint); - - Vector2 he( - (local_v.x<0) ? -half_extents.x : half_extents.x, - (local_v.y<0) ? -half_extents.y : half_extents.y - ); - - if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized())) + if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint))) return; } @@ -1011,13 +1078,13 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_ separator.generate_contacts(); } -template -static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const RectangleShape2DSW *rectangle_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -1033,10 +1100,36 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri return; //convex faces + Matrix32 boxinv; + if (withMargin) { + boxinv=p_transform_a.affine_inverse(); + } for(int i=0;iget_point_count();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))))) + 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))) + 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))) + 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))) + return; + } + + } } separator.generate_contacts(); @@ -1046,14 +1139,14 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri ///////// -template -static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CapsuleShape2DSW *capsule_A = static_cast(p_a); const CapsuleShape2DSW *capsule_B = static_cast(p_b); - SeparatorAxisTest2D separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -1089,14 +1182,14 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr } -template -static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const CapsuleShape2DSW *capsule_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -1135,14 +1228,14 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3 ///////// -template -static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) { +template +static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) { const ConvexPolygonShape2DSW *convex_A = static_cast(p_a); const ConvexPolygonShape2DSW *convex_B = static_cast(p_b); - SeparatorAxisTest2D separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b); + SeparatorAxisTest2D 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; @@ -1161,6 +1254,19 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i))) return; + + } + + if (withMargin) { + + for(int i=0;iget_point_count();i++) { + for(int j=0;jget_point_count();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(); @@ -1170,7 +1276,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const //////// -bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) { +bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,float p_margin_A,float p_margin_B) { Physics2DServer::ShapeType type_A=p_shape_A->get_type(); @@ -1186,121 +1292,238 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_ static const CollisionFunc collision_table[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; static const CollisionFunc collision_table_castA[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; static const CollisionFunc collision_table_castB[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; static const CollisionFunc collision_table_castA_castB[5][5]={ - {_collision_segment_segment, - _collision_segment_circle, - _collision_segment_rectangle, - _collision_segment_capsule, - _collision_segment_convex_polygon}, + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, - _collision_circle_circle, - _collision_circle_rectangle, - _collision_circle_capsule, - _collision_circle_convex_polygon}, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castA_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castB_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, + {0, + 0, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, + {0, + 0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, + {0, + 0, + 0, + 0, + _collision_convex_polygon_convex_polygon} + + }; + + static const CollisionFunc collision_table_castA_castB_margin[5][5]={ + {_collision_segment_segment, + _collision_segment_circle, + _collision_segment_rectangle, + _collision_segment_capsule, + _collision_segment_convex_polygon}, + {0, + _collision_circle_circle, + _collision_circle_rectangle, + _collision_circle_capsule, + _collision_circle_convex_polygon}, {0, 0, - _collision_rectangle_rectangle, - _collision_rectangle_capsule, - _collision_rectangle_convex_polygon}, + _collision_rectangle_rectangle, + _collision_rectangle_capsule, + _collision_rectangle_convex_polygon}, {0, 0, 0, - _collision_capsule_capsule, - _collision_capsule_convex_polygon}, + _collision_capsule_capsule, + _collision_capsule_convex_polygon}, {0, 0, 0, 0, - _collision_convex_polygon_convex_polygon} + _collision_convex_polygon_convex_polygon} }; + _CollectorCallback2D callback; callback.callback=p_result_callback; callback.swap=p_swap; @@ -1314,32 +1537,49 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_ const Matrix32 *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); callback.swap = !callback.swap; } CollisionFunc collision_func; - 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 (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]; + } else { + collision_func = collision_table_castA_castB_margin[type_A-2][type_B-2]; + } } else { - collision_func = collision_table_castA_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]; + } + } ERR_FAIL_COND_V(!collision_func,false); - collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B); + collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B,margin_A,margin_B); return callback.collided; -- cgit v1.2.3