3 files changed, 40 insertions, 31 deletions

diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index ae549ed2e4..ec74507e03 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -29,6 +29,7 @@
 /*************************************************************************/
 
 #include "broad_phase_2d_hash_grid.h"
+#include "collision_object_2d_sw.h"
 #include "core/project_settings.h"
 
 #define LARGE_ELEMENT_FI 1.01239812
@@ -70,20 +71,22 @@ void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) {
 
 void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
 	for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {
-		bool pairing = p_elem->aabb.intersects(E->key()->aabb);
-
-		if (pairing != E->get()->colliding) {
-			if (pairing) {
-				if (pair_callback) {
-					E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
-				}
-			} else {
-				if (unpair_callback) {
-					unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
-				}
+		bool physical_collision = p_elem->aabb.intersects(E->key()->aabb);
+		bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner);
+
+		if (physical_collision) {
+			if (!E->get()->colliding || (logical_collision && !E->get()->ud && pair_callback)) {
+				E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
+			} else if (E->get()->colliding && !logical_collision && E->get()->ud && unpair_callback) {
+				unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
+				E->get()->ud = nullptr;
 			}
-
-			E->get()->colliding = pairing;
+			E->get()->colliding = true;
+		} else { // No physcial_collision
+			if (E->get()->colliding && unpair_callback) {
+				unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
+			}
+			E->get()->colliding = false;
 		}
 	}
 }
@@ -317,23 +320,17 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
 
 	Element &e = E->get();
 
-	if (p_aabb == e.aabb) {
-		return;
-	}
-
-	if (p_aabb != Rect2()) {
-		_enter_grid(&e, p_aabb, e._static);
-	}
-
-	if (e.aabb != Rect2()) {
-		_exit_grid(&e, e.aabb, e._static);
+	if (p_aabb != e.aabb) {
+		if (p_aabb != Rect2()) {
+			_enter_grid(&e, p_aabb, e._static);
+		}
+		if (e.aabb != Rect2()) {
+			_exit_grid(&e, e.aabb, e._static);
+		}
+		e.aabb = p_aabb;
 	}
 
-	e.aabb = p_aabb;
-
 	_check_motion(&e);
-
-	e.aabb = p_aabb;
 }
 
 void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 84a2baaa74..8caa53680d 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -168,10 +168,16 @@ public:
 		return shapes[p_idx].one_way_collision_margin;
 	}
 
-	void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
+	void set_collision_mask(uint32_t p_mask) {
+		collision_mask = p_mask;
+		_shape_changed();
+	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
-	void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; }
+	void set_collision_layer(uint32_t p_layer) {
+		collision_layer = p_layer;
+		_shape_changed();
+	}
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
 
 	void remove_shape(Shape2DSW *p_shape);
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h
index 9506f14402..a3a5787ced 100644
--- a/servers/physics_3d/collision_object_3d_sw.h
+++ b/servers/physics_3d/collision_object_3d_sw.h
@@ -142,10 +142,16 @@ public:
 		return shapes[p_idx].disabled;
 	}
 
-	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; }
+	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
+		collision_layer = p_layer;
+		_shape_changed();
+	}
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
 
-	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
+	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
+		collision_mask = p_mask;
+		_shape_changed();
+	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
 	_FORCE_INLINE_ bool test_collision_mask(CollisionObject3DSW *p_other) const {