4 files changed, 50 insertions, 5 deletions
diff --git a/core/math/aabb.h b/core/math/aabb.h
index 474304eae2..24908ae59d 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -107,6 +107,9 @@ public:
 	Variant intersects_segment_bind(const Vector3 &p_from, const Vector3 &p_to) const;
 	Variant intersects_ray_bind(const Vector3 &p_from, const Vector3 &p_dir) const;
 
+	_FORCE_INLINE_ void quantize(float p_unit);
+	_FORCE_INLINE_ AABB quantized(float p_unit) const;
+
 	_FORCE_INLINE_ void set_end(const Vector3 &p_end) {
 		size = p_end - position;
 	}
@@ -427,4 +430,28 @@ void AABB::grow_by(real_t p_amount) {
 	size.z += 2.0 * p_amount;
 }
 
+void AABB::quantize(float p_unit) {
+	size += position;
+
+	position.x -= Math::fposmodp(position.x, p_unit);
+	position.y -= Math::fposmodp(position.y, p_unit);
+	position.z -= Math::fposmodp(position.z, p_unit);
+
+	size.x -= Math::fposmodp(size.x, p_unit);
+	size.y -= Math::fposmodp(size.y, p_unit);
+	size.z -= Math::fposmodp(size.z, p_unit);
+
+	size.x += p_unit;
+	size.y += p_unit;
+	size.z += p_unit;
+
+	size -= position;
+}
+
+AABB AABB::quantized(float p_unit) const {
+	AABB ret = *this;
+	ret.quantize(p_unit);
+	return ret;
+}
+
 #endif // AABB_H
diff --git a/core/math/dynamic_bvh.cpp b/core/math/dynamic_bvh.cpp
index a53f8d1eb8..e66de5037d 100644
--- a/core/math/dynamic_bvh.cpp
+++ b/core/math/dynamic_bvh.cpp
@@ -351,17 +351,17 @@ void DynamicBVH::_update(Node *leaf, int lookahead) {
 	_insert_leaf(root, leaf);
 }
 
-void DynamicBVH::update(const ID &p_id, const AABB &p_box) {
-	ERR_FAIL_COND(!p_id.is_valid());
+bool DynamicBVH::update(const ID &p_id, const AABB &p_box) {
+	ERR_FAIL_COND_V(!p_id.is_valid(), false);
 	Node *leaf = p_id.node;
 
 	Volume volume;
 	volume.min = p_box.position;
 	volume.max = p_box.position + p_box.size;
 
-	if ((leaf->volume.min == volume.min) && (leaf->volume.max == volume.max)) {
+	if (leaf->volume.min.is_equal_approx(volume.min) && leaf->volume.max.is_equal_approx(volume.max)) {
 		// noop
-		return;
+		return false;
 	}
 
 	Node *base = _remove_leaf(leaf);
@@ -375,6 +375,7 @@ void DynamicBVH::update(const ID &p_id, const AABB &p_box) {
 	}
 	leaf->volume = volume;
 	_insert_leaf(base, leaf);
+	return true;
 }
 
 void DynamicBVH::remove(const ID &p_id) {
diff --git a/core/math/dynamic_bvh.h b/core/math/dynamic_bvh.h
index cdea51c674..968badc093 100644
--- a/core/math/dynamic_bvh.h
+++ b/core/math/dynamic_bvh.h
@@ -281,7 +281,7 @@ public:
 	void optimize_top_down(int bu_threshold = 128);
 	void optimize_incremental(int passes);
 	ID insert(const AABB &p_box, void *p_userdata);
-	void update(const ID &p_id, const AABB &p_box);
+	bool update(const ID &p_id, const AABB &p_box);
 	void remove(const ID &p_id);
 	void get_elements(List<ID> *r_elements);
 
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 827637bf2b..471aa58996 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -198,6 +198,23 @@ public:
 		value += 0.0;
 		return value;
 	}
+	static _ALWAYS_INLINE_ float fposmodp(float p_x, float p_y) {
+		float value = Math::fmod(p_x, p_y);
+		if (value < 0) {
+			value += p_y;
+		}
+		value += 0.0;
+		return value;
+	}
+	static _ALWAYS_INLINE_ double fposmodp(double p_x, double p_y) {
+		double value = Math::fmod(p_x, p_y);
+		if (value < 0) {
+			value += p_y;
+		}
+		value += 0.0;
+		return value;
+	}
+
 	static _ALWAYS_INLINE_ int posmod(int p_x, int p_y) {
 		int value = p_x % p_y;
 		if ((value < 0 && p_y > 0) || (value > 0 && p_y < 0)) {