-added kinematic body

-added kinematic body demos

1 files changed, 431 insertions, 191 deletions

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<class ShapeA, class ShapeB,bool castA=false,bool castB=false>
+template<class ShapeA, class ShapeB,bool castA=false,bool castB=false, bool withMargin=false>
 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;i<support_count_A;i++) {
+				supports_A[i]+=-best_axis*margin_A;
+			}
+
+		}
+
 
 
 		Vector2 supports_B[max_supports];
@@ -480,6 +497,15 @@ public:
 				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;
+			}
+
+		}
+
 /*
 
 
@@ -517,7 +543,10 @@ public:
 
 	}
 
-	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2()) {
+	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& 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;
@@ -548,16 +577,16 @@ public:
 		(castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) )
 
 
-typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&);
+typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&,float,float);
 
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const SegmentShape2DSW*>(p_a);
 	const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const SegmentShape2DSW*>(p_a);
 	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -612,13 +656,13 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_tra
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const SegmentShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const SegmentShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -671,13 +753,13 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const SegmentShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const CircleShape2DSW*>(p_a);
 	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -724,14 +816,14 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_tran
 
 }
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const CircleShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const CircleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -840,14 +902,14 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_tra
 
 }
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const CircleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -872,14 +934,14 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32
 
 /////////
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const RectangleShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const RectangleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const RectangleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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;i<convex_B->get_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<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const CapsuleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -1089,14 +1182,14 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 
 }
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const CapsuleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -1135,14 +1228,14 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
 /////////
 
 
-template<bool castA, bool castB>
-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<bool castA, bool castB,bool withMargin>
+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<const ConvexPolygonShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_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;
@@ -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;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))))
+					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<false,false>,
-		 _collision_segment_circle<false,false>,
-		 _collision_segment_rectangle<false,false>,
-		 _collision_segment_capsule<false,false>,
-		 _collision_segment_convex_polygon<false,false>},
+		{_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>,
-		 _collision_circle_rectangle<false,false>,
-		 _collision_circle_capsule<false,false>,
-		 _collision_circle_convex_polygon<false,false>},
+		 _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>,
-		 _collision_rectangle_capsule<false,false>,
-		 _collision_rectangle_convex_polygon<false,false>},
+		 _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>,
-		 _collision_capsule_convex_polygon<false,false>},
+		 _collision_capsule_capsule<false,false,false>,
+		 _collision_capsule_convex_polygon<false,false,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<false,false>}
+		 _collision_convex_polygon_convex_polygon<false,false,false>}
 
 	};
 
 	static const CollisionFunc collision_table_castA[5][5]={
-		{_collision_segment_segment<true,false>,
-		 _collision_segment_circle<true,false>,
-		 _collision_segment_rectangle<true,false>,
-		 _collision_segment_capsule<true,false>,
-		 _collision_segment_convex_polygon<true,false>},
+		{_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>,
-		 _collision_circle_rectangle<true,false>,
-		 _collision_circle_capsule<true,false>,
-		 _collision_circle_convex_polygon<true,false>},
+		 _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>,
-		 _collision_rectangle_capsule<true,false>,
-		 _collision_rectangle_convex_polygon<true,false>},
+		 _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>,
-		 _collision_capsule_convex_polygon<true,false>},
+		 _collision_capsule_capsule<true,false,false>,
+		 _collision_capsule_convex_polygon<true,false,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<true,false>}
+		 _collision_convex_polygon_convex_polygon<true,false,false>}
 
 	};
 
 	static const CollisionFunc collision_table_castB[5][5]={
-		{_collision_segment_segment<false,true>,
-		 _collision_segment_circle<false,true>,
-		 _collision_segment_rectangle<false,true>,
-		 _collision_segment_capsule<false,true>,
-		 _collision_segment_convex_polygon<false,true>},
+		{_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>,
-		 _collision_circle_rectangle<false,true>,
-		 _collision_circle_capsule<false,true>,
-		 _collision_circle_convex_polygon<false,true>},
+		 _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>,
-		 _collision_rectangle_capsule<false,true>,
-		 _collision_rectangle_convex_polygon<false,true>},
+		 _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>,
-		 _collision_capsule_convex_polygon<false,true>},
+		 _collision_capsule_capsule<false,true,false>,
+		 _collision_capsule_convex_polygon<false,true,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<false,true>}
+		 _collision_convex_polygon_convex_polygon<false,true,false>}
 
 	};
 
 	static const CollisionFunc collision_table_castA_castB[5][5]={
-		{_collision_segment_segment<true,true>,
-		 _collision_segment_circle<true,true>,
-		 _collision_segment_rectangle<true,true>,
-		 _collision_segment_capsule<true,true>,
-		 _collision_segment_convex_polygon<true,true>},
+		{_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,
-		 _collision_circle_circle<true,true>,
-		 _collision_circle_rectangle<true,true>,
-		 _collision_circle_capsule<true,true>,
-		 _collision_circle_convex_polygon<true,true>},
+		 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_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>,
-		 _collision_rectangle_capsule<true,true>,
-		 _collision_rectangle_convex_polygon<true,true>},
+		 _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>,
-		 _collision_capsule_convex_polygon<true,true>},
+		 _collision_capsule_capsule<true,true,true>,
+		 _collision_capsule_convex_polygon<true,true,true>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<true,true>}
+		 _collision_convex_polygon_convex_polygon<true,true,true>}
 
 	};
 
+
 	_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;