summaryrefslogtreecommitdiff
path: root/servers/physics_2d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d')
-rw-r--r--servers/physics_2d/body_2d_sw.h1
-rw-r--r--servers/physics_2d/broad_phase_2d_hash_grid.cpp3
-rw-r--r--servers/physics_2d/collision_object_2d_sw.cpp1
-rw-r--r--servers/physics_2d/collision_object_2d_sw.h3
-rw-r--r--servers/physics_2d/collision_solver_2d_sat.cpp622
-rw-r--r--servers/physics_2d/collision_solver_2d_sat.h2
-rw-r--r--servers/physics_2d/collision_solver_2d_sw.cpp18
-rw-r--r--servers/physics_2d/collision_solver_2d_sw.h4
-rw-r--r--servers/physics_2d/physics_2d_server_sw.cpp12
-rw-r--r--servers/physics_2d/physics_2d_server_sw.h13
-rw-r--r--servers/physics_2d/shape_2d_sw.h45
-rw-r--r--servers/physics_2d/space_2d_sw.cpp307
-rw-r--r--servers/physics_2d/space_2d_sw.h8
13 files changed, 701 insertions, 338 deletions
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 14cae3dbb0..ffe47e0267 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -62,6 +62,7 @@ class Body2DSW : public CollisionObject2DSW {
Vector2 applied_force;
real_t applied_torque;
+
SelfList<Body2DSW> active_list;
SelfList<Body2DSW> inertia_update_list;
SelfList<Body2DSW> direct_state_query_list;
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 129c9ecb9c..0f08f63937 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -434,8 +434,9 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const
if (E->key()->pass==pass)
continue;
- if (use_aabb && !p_aabb.intersects(E->key()->aabb))
+ if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
continue;
+ }
if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
continue;
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 3a5c8c3ade..e07dca472b 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -218,4 +218,5 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
type=p_type;
space=NULL;
instance_id=0;
+ user_mask=0;
}
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 74457cfa0a..8138cfcc69 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -65,6 +65,7 @@ private:
Space2DSW *space;
Matrix32 transform;
Matrix32 inv_transform;
+ uint32_t user_mask;
bool _static;
void _update_shapes();
@@ -117,6 +118,8 @@ public:
_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
+ void set_user_mask(uint32_t p_mask) {user_mask=p_mask;}
+ _FORCE_INLINE_ uint32_t get_user_mask() const { return user_mask; }
void remove_shape(Shape2DSW *p_shape);
void remove_shape(int p_index);
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;
diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h
index 95468a18b8..be5a3dc79f 100644
--- a/servers/physics_2d/collision_solver_2d_sat.h
+++ b/servers/physics_2d/collision_solver_2d_sat.h
@@ -32,6 +32,6 @@
#include "collision_solver_2d_sw.h"
-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=false,Vector2 *sep_axis=NULL);
+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=false,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
#endif // COLLISION_SOLVER_2D_SAT_H
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index 5d43510aea..4c7e68d643 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -150,6 +150,8 @@ struct _ConcaveCollisionInfo2D {
const Matrix32 *transform_B;
Vector2 motion_A;
Vector2 motion_B;
+ real_t margin_A;
+ real_t margin_B;
CollisionSolver2DSW::CallbackResult result_callback;
void *userdata;
bool swap_result;
@@ -169,7 +171,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
if (!cinfo.result_callback && cinfo.collided)
return; //already collided and no contacts requested, don't test anymore
- bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
+ bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis,cinfo.margin_A,cinfo.margin_B );
if (!collided)
return;
@@ -179,7 +181,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
}
-bool CollisionSolver2DSW::solve_concave(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,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
+bool CollisionSolver2DSW::solve_concave(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,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
@@ -195,6 +197,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
cinfo.collided=false;
cinfo.collisions=0;
cinfo.sep_axis=sep_axis;
+ cinfo.margin_A=p_margin_A;
+ cinfo.margin_B=p_margin_B;
cinfo.aabb_tests=0;
@@ -227,7 +231,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
}
-bool CollisionSolver2DSW::solve(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,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
+bool CollisionSolver2DSW::solve(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,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
@@ -236,12 +240,14 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
Physics2DServer::ShapeType type_B=p_shape_B->get_type();
bool concave_A=p_shape_A->is_concave();
bool concave_B=p_shape_B->is_concave();
+ real_t margin_A=p_margin_A,margin_B=p_margin_B;
bool swap = false;
if (type_A>type_B) {
SWAP(type_A,type_B);
SWAP(concave_A,concave_B);
+ SWAP(margin_A,margin_B);
swap=true;
}
@@ -292,16 +298,16 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
return false;
if (!swap)
- return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis);
+ return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
else
- return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis);
+ return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis,margin_A,margin_B);
} else {
- return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis);
+ return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
}
diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h
index 11b5701f46..07141b0d09 100644
--- a/servers/physics_2d/collision_solver_2d_sw.h
+++ b/servers/physics_2d/collision_solver_2d_sw.h
@@ -37,14 +37,14 @@ public:
private:
static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
- static bool solve_concave(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,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
+ static bool solve_concave(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,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
public:
- static bool solve(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,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
+ static bool solve(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,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
};
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 0fbd461f46..fd1ea579f0 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -132,8 +132,12 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
+
CollCbkData *cbk=(CollCbkData *)p_userdata;
+ if (cbk->max==0)
+ return;
+
if (cbk->amount == cbk->max) {
//find least deep
float min_depth=1e20;
@@ -159,6 +163,7 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p
cbk->ptr[cbk->amount*2+0]=p_point_A;
cbk->ptr[cbk->amount*2+1]=p_point_B;
+ cbk->amount++;
}
}
@@ -648,19 +653,20 @@ uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const {
};
-void Physics2DServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
+void Physics2DServerSW::body_set_user_mask(RID p_body, uint32_t p_flags) {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND(!body);
+ body->set_user_mask(p_flags);
};
-uint32_t Physics2DServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const {
+uint32_t Physics2DServerSW::body_get_user_mask(RID p_body, uint32_t p_flags) const {
Body2DSW *body = body_owner.get(p_body);
ERR_FAIL_COND_V(!body,0);
- return 0;
+ return body->get_user_mask();
};
void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index ef00eae7e4..e50bb0ab96 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -58,6 +58,12 @@ friend class Physics2DDirectSpaceStateSW;
mutable RID_Owner<Body2DSW> body_owner;
mutable RID_Owner<Joint2DSW> joint_owner;
+
+
+
+// void _clear_query(Query2DSW *p_query);
+public:
+
struct CollCbkData {
int max;
@@ -68,9 +74,6 @@ friend class Physics2DDirectSpaceStateSW;
static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
-// void _clear_query(Query2DSW *p_query);
-public:
-
virtual RID shape_create(ShapeType p_shape);
virtual void shape_set_data(RID p_shape, const Variant& p_data);
virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
@@ -158,8 +161,8 @@ public:
virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode);
virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const;
- virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
- virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
+ virtual void body_set_user_mask(RID p_body, uint32_t p_mask);
+ virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const;
virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
virtual float body_get_param(RID p_body, BodyParameter p_param) const;
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index ba5f60cb32..d3fcf1fab2 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -354,6 +354,51 @@ public:
r_max = distance + length;
}
+
+
+ _FORCE_INLINE_ Vector2 get_circle_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const Vector2& p_circle) const {
+
+ Vector2 local_v = p_xform_inv.xform(p_circle);
+
+ Vector2 he(
+ (local_v.x<0) ? -half_extents.x : half_extents.x,
+ (local_v.y<0) ? -half_extents.y : half_extents.y
+ );
+
+ return (p_xform.xform(he)-p_circle).normalized();
+ }
+
+ _FORCE_INLINE_ Vector2 get_box_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const RectangleShape2DSW *p_B,const Matrix32& p_B_xform, const Matrix32& p_B_xform_inv) const {
+
+ Vector2 a,b;
+
+ {
+ Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin());
+
+ Vector2 he(
+ (local_v.x<0) ? -half_extents.x : half_extents.x,
+ (local_v.y<0) ? -half_extents.y : half_extents.y
+ );
+
+ a=p_xform.xform(he);
+
+ }
+ {
+ Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin());
+
+ Vector2 he(
+ (local_v.x<0) ? -p_B->half_extents.x : p_B->half_extents.x,
+ (local_v.y<0) ? -p_B->half_extents.y : p_B->half_extents.y
+ );
+
+ b=p_B_xform.xform(he);
+
+ }
+
+ return (a-b).normalized();
+ }
+
+
DEFAULT_PROJECT_RANGE_CAST
};
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 2c714f5065..d1aec92984 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -31,7 +31,22 @@
#include "physics_2d_server_sw.h"
-bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+_FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_user_mask, uint32_t p_type_mask) {
+
+ if (p_user_mask && !(p_object->get_user_mask()&p_user_mask))
+ return false;
+
+ if (p_object->get_type()==CollisionObject2DSW::TYPE_AREA && !(p_type_mask&Physics2DDirectSpaceState::TYPE_MASK_AREA))
+ return false;
+
+ Body2DSW *body = static_cast<Body2DSW*>(p_object);
+
+ return (1<<body->get_mode())&p_type_mask;
+
+}
+
+bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
+
ERR_FAIL_COND_V(space->locked,false);
@@ -55,8 +70,8 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
for(int i=0;i<amount;i++) {
- if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
- continue; //ignore area
+ if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+ continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
@@ -120,7 +135,7 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
}
-int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
if (p_result_max<=0)
return 0;
@@ -129,6 +144,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
ERR_FAIL_COND_V(!shape,0);
Rect2 aabb = p_xform.xform(shape->get_aabb());
+ aabb=aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
@@ -137,11 +153,8 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
for(int i=0;i<amount;i++) {
- if (cc>=p_result_max)
- break;
-
- if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
- continue; //ignore area
+ if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+ continue;
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
continue;
@@ -150,7 +163,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
- if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL))
+ if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL,p_margin))
continue;
r_results[cc].collider_id=col_obj->get_instance_id();
@@ -168,193 +181,235 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
}
-struct MotionCallbackRayCastData {
- Vector2 best_contact;
- Vector2 best_normal;
- float best_len;
- Matrix32 b_xform_inv;
- Matrix32 b_xform;
- Vector2 motion;
- Shape2DSW * shape_B;
+bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
-};
-static void _motion_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
+ Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,false);
+
+ Rect2 aabb = p_xform.xform(shape->get_aabb());
+ aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+ aabb=aabb.grow(p_margin);
- MotionCallbackRayCastData *rd=(MotionCallbackRayCastData*)p_userdata;
+ //if (p_motion!=Vector2())
+ // print_line(p_motion);
- Vector2 contact_normal = (p_point_B-p_point_A).normalized();
+ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
- Vector2 from=p_point_A-(rd->motion*1.01);
- Vector2 p,n;
+ float best_safe=1;
+ float best_unsafe=1;
+
+ for(int i=0;i<amount;i++) {
- if (contact_normal.dot(rd->motion.normalized())<CMP_EPSILON) {
- //safe to assume it was a perpendicular collision
- n=contact_normal;
- p=p_point_B;
- } else {
- //entered in a different angle
- Vector2 to = p_point_A+rd->motion; //avoid precission issues
+ if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+ continue;
+
+ if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+ continue; //ignore excluded
- bool res = rd->shape_B->intersect_segment(rd->b_xform_inv.xform(from),rd->b_xform_inv.xform(to),p,n);
+ const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
- if (!res) {
- print_line("lolwut failed");
- return;
+
+ Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+ //test initial overlap, does it collide if going all the way?
+ if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
+ continue;
}
- p = rd->b_xform.xform(p);
- n = rd->b_xform_inv.basis_xform_inv(n).normalized();
- }
+ //test initial overlap
+ if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
+
+ return false;
+ }
+
+
+ //just do kinematic solving
+ float low=0;
+ float hi=1;
+ Vector2 mnormal=p_motion.normalized();
+
+ for(int i=0;i<8;i++) { //steps should be customizable..
+
+ Matrix32 xfa = p_xform;
+ float ofs = (low+hi)*0.5;
+
+ Vector2 sep=mnormal; //important optimization for this to work fast enough
+ bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin);
+
+ if (collided) {
+
+ hi=ofs;
+ } else {
+
+ low=ofs;
+ }
+ }
- float len = p.distance_to(from);
+ if (low<best_safe) {
+ best_safe=low;
+ best_unsafe=hi;
+ }
- if (len<rd->best_len) {
- rd->best_contact=p;
- rd->best_normal=n;
- rd->best_len=len;
}
+
+ p_closest_safe=best_safe;
+ p_closest_unsafe=best_unsafe;
+
+ return true;
+
+
}
-bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
+
+
+ if (p_result_max<=0)
+ return 0;
Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape,0);
- Rect2 aabb = p_xform.xform(shape->get_aabb());
+ Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+ aabb=aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
bool collided=false;
- r_result.travel=1;
-
- MotionCallbackRayCastData best_normal;
- best_normal.best_len=1e20;
- for(int i=0;i<amount;i++) {
-
+ int cc=0;
+ r_result_count=0;
+
+ Physics2DServerSW::CollCbkData cbk;
+ cbk.max=p_result_max;
+ cbk.amount=0;
+ cbk.ptr=r_results;
+ CollisionSolver2DSW::CallbackResult cbkres=NULL;
+
+ Physics2DServerSW::CollCbkData *cbkptr=NULL;
+ if (p_result_max>0) {
+ cbkptr=&cbk;
+ cbkres=Physics2DServerSW::_shape_col_cbk;
+ }
- if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
- continue; //ignore area
- if (p_exclude.has( space->intersection_query_results[i]->get_self()))
- continue; //ignore excluded
+ for(int i=0;i<amount;i++) {
+ if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+ continue;
const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
int shape_idx=space->intersection_query_subindex_results[i];
+ if (p_exclude.has( col_obj->get_self() ))
+ continue;
- Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
- //test initial overlap, does it collide if going all the way?
- if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
- continue;
+ if (CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
+ collided=true;
}
+ }
- //test initial overlap
- if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
-
- r_result.collider_id=col_obj->get_instance_id();
- r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
- r_result.shape=shape_idx;
- r_result.rid=col_obj->get_self();
- r_result.travel=0;
- r_result.point=Vector2();
- r_result.normal=Vector2();
- return true;
- }
+ r_result_count=cbk.amount;
-#if 0
- Vector2 mnormal=p_motion.normalized();
- Matrix32 col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
- ShapeSW *col_shape = col_obj->get_shape(shape_idx);
+ return collided;
+}
- real_t min,max;
- col_shape->project_rangev(mnormal,col_shape_xform,min,max);
- real_t width = max-min;
- int a;
- Vector2 s[2];
- col_shape->get_supports(col_shape_xform.basis_xform(mnormal).normalized(),s,a);
- Vector2 from = col_shape_xform.xform(s[0]);
- Vector2 to = from + p_motion;
+struct _RestCallbackData {
- Matrix32 from_inv = col_shape_xform.affine_inverse();
+ const CollisionObject2DSW *object;
+ const CollisionObject2DSW *best_object;
+ int shape;
+ int best_shape;
+ Vector2 best_contact;
+ Vector2 best_normal;
+ float best_len;
+};
- Vector2 local_from = from_inv.xform(from-mnormal*width*0.1); //start from a little inside the bounding box
- Vector2 local_to = from_inv.xform(to);
+static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
- Vector2 rpos,rnorm;
- if (!col_shape->intersect_segment(local_from,local_to,rpos,rnorm))
- return false;
- //ray hit something
+ _RestCallbackData *rd=(_RestCallbackData*)p_userdata;
+ Vector2 contact_rel = p_point_B - p_point_A;
+ float len = contact_rel.length();
+ if (len <= rd->best_len)
+ return;
- Vector2 hitpos = p_xform_B.xform(rpos);
-#endif
+ rd->best_len=len;
+ rd->best_contact=p_point_B;
+ rd->best_normal=contact_rel/len;
+ rd->best_object=rd->object;
+ rd->best_shape=rd->shape;
- //just do kinematic solving
- float low=0;
- float hi=1;
- Vector2 mnormal=p_motion.normalized();
+}
- for(int i=0;i<8;i++) { //steps should be customizable..
- Matrix32 xfa = p_xform;
- float ofs = (low+hi)*0.5;
+bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
- Vector2 sep=mnormal; //important optimization for this to work fast enough
- bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep);
- if (collided) {
+ Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,0);
- hi=ofs;
- } else {
+ Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
+ aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+ aabb=aabb.grow(p_margin);
- low=ofs;
- }
- }
+ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+ _RestCallbackData rcd;
+ rcd.best_len=0;
+ rcd.best_object=NULL;
+ rcd.best_shape=0;
+ for(int i=0;i<amount;i++) {
- best_normal.shape_B=col_obj->get_shape(shape_idx);
- best_normal.motion=p_motion*hi;
- best_normal.b_xform=col_obj_xform;
- best_normal.b_xform_inv=col_obj_xform.affine_inverse();
- bool sc = CollisionSolver2DSW::solve(shape,p_xform,p_motion*hi,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_motion_cbk_result,&best_normal);
- print_line("CLD: "+itos(sc));
+ if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+ continue;
+ const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
- if (collided && low>=r_result.travel)
+ if (p_exclude.has( col_obj->get_self() ))
continue;
- collided=true;
- r_result.travel=low;
+ rcd.object=col_obj;
+ rcd.shape=shape_idx;
+ bool sc = CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
+ if (!sc)
+ continue;
- r_result.collider_id=col_obj->get_instance_id();
- r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
- r_result.shape=shape_idx;
- r_result.rid=col_obj->get_self();
}
- if (collided) {
- ERR_FAIL_COND_V(best_normal.best_normal==Vector2(),false);
- r_result.normal=best_normal.best_normal;
- r_result.point=best_normal.best_contact;
- }
+ if (rcd.best_len==0)
+ return false;
- return collided;
+ r_info->collider_id=rcd.best_object->get_instance_id();
+ r_info->shape=rcd.best_shape;
+ r_info->normal=rcd.best_normal;
+ r_info->point=rcd.best_contact;
+ r_info->rid=rcd.best_object->get_self();
+ if (rcd.best_object->get_type()==CollisionObject2DSW::TYPE_BODY) {
+ const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
+ Vector2 rel_vec = r_info->point-body->get_transform().get_origin();
+ r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+ } else {
+ r_info->linear_velocity=Vector2();
+ }
+
+ return true;
}
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 978e88479d..9d3dfae9b5 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -46,9 +46,11 @@ public:
Space2DSW *space;
- bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
- int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
- bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+ virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+ virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
Physics2DDirectSpaceStateSW();
};
generated by cgit v1.2.3 (git 2.25.1) at 2025-02-02 06:00:11 +0000