1 files changed, 129 insertions, 63 deletions

diff --git a/core/math/aabb.h b/core/math/aabb.h
index 7fdad07c89..2861358e32 100644
--- a/core/math/aabb.h
+++ b/core/math/aabb.h
@@ -5,8 +5,8 @@
 /*                           GODOT ENGINE                                */
 /*                      https://godotengine.org                          */
 /*************************************************************************/
-/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md).   */
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
 /*                                                                       */
 /* Permission is hereby granted, free of charge, to any person obtaining */
 /* a copy of this software and associated documentation files (the       */
@@ -39,6 +39,7 @@
  * AABB / AABB (Axis Aligned Bounding Box)
  * This is implemented by a point (position) and the box size
  */
+class Variant;
 
 class AABB {
 public:
@@ -47,12 +48,10 @@ public:
 
 	real_t get_area() const; /// get area
 	_FORCE_INLINE_ bool has_no_area() const {
-
 		return (size.x <= 0 || size.y <= 0 || size.z <= 0);
 	}
 
 	_FORCE_INLINE_ bool has_no_surface() const {
-
 		return (size.x <= 0 && size.y <= 0 && size.z <= 0);
 	}
 
@@ -101,6 +100,24 @@ public:
 	_FORCE_INLINE_ void project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const;
 	_FORCE_INLINE_ void expand_to(const Vector3 &p_vector); /** expand to contain a point if necessary */
 
+	_FORCE_INLINE_ AABB abs() const {
+		return AABB(Vector3(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0), position.z + MIN(size.z, 0)), size.abs());
+	}
+
+	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;
+	}
+
+	_FORCE_INLINE_ Vector3 get_end() const {
+		return position + size;
+	}
+
 	operator String() const;
 
 	_FORCE_INLINE_ AABB() {}
@@ -111,43 +128,52 @@ public:
 };
 
 inline bool AABB::intersects(const AABB &p_aabb) const {
-
-	if (position.x >= (p_aabb.position.x + p_aabb.size.x))
+	if (position.x >= (p_aabb.position.x + p_aabb.size.x)) {
 		return false;
-	if ((position.x + size.x) <= p_aabb.position.x)
+	}
+	if ((position.x + size.x) <= p_aabb.position.x) {
 		return false;
-	if (position.y >= (p_aabb.position.y + p_aabb.size.y))
+	}
+	if (position.y >= (p_aabb.position.y + p_aabb.size.y)) {
 		return false;
-	if ((position.y + size.y) <= p_aabb.position.y)
+	}
+	if ((position.y + size.y) <= p_aabb.position.y) {
 		return false;
-	if (position.z >= (p_aabb.position.z + p_aabb.size.z))
+	}
+	if (position.z >= (p_aabb.position.z + p_aabb.size.z)) {
 		return false;
-	if ((position.z + size.z) <= p_aabb.position.z)
+	}
+	if ((position.z + size.z) <= p_aabb.position.z) {
 		return false;
+	}
 
 	return true;
 }
 
 inline bool AABB::intersects_inclusive(const AABB &p_aabb) const {
-
-	if (position.x > (p_aabb.position.x + p_aabb.size.x))
+	if (position.x > (p_aabb.position.x + p_aabb.size.x)) {
 		return false;
-	if ((position.x + size.x) < p_aabb.position.x)
+	}
+	if ((position.x + size.x) < p_aabb.position.x) {
 		return false;
-	if (position.y > (p_aabb.position.y + p_aabb.size.y))
+	}
+	if (position.y > (p_aabb.position.y + p_aabb.size.y)) {
 		return false;
-	if ((position.y + size.y) < p_aabb.position.y)
+	}
+	if ((position.y + size.y) < p_aabb.position.y) {
 		return false;
-	if (position.z > (p_aabb.position.z + p_aabb.size.z))
+	}
+	if (position.z > (p_aabb.position.z + p_aabb.size.z)) {
 		return false;
-	if ((position.z + size.z) < p_aabb.position.z)
+	}
+	if ((position.z + size.z) < p_aabb.position.z) {
 		return false;
+	}
 
 	return true;
 }
 
 inline bool AABB::encloses(const AABB &p_aabb) const {
-
 	Vector3 src_min = position;
 	Vector3 src_max = position + size;
 	Vector3 dst_min = p_aabb.position;
@@ -163,35 +189,40 @@ inline bool AABB::encloses(const AABB &p_aabb) const {
 }
 
 Vector3 AABB::get_support(const Vector3 &p_normal) const {
-
 	Vector3 half_extents = size * 0.5;
 	Vector3 ofs = position + half_extents;
 
 	return Vector3(
-				   (p_normal.x > 0) ? -half_extents.x : half_extents.x,
-				   (p_normal.y > 0) ? -half_extents.y : half_extents.y,
-				   (p_normal.z > 0) ? -half_extents.z : half_extents.z) +
+				   (p_normal.x > 0) ? half_extents.x : -half_extents.x,
+				   (p_normal.y > 0) ? half_extents.y : -half_extents.y,
+				   (p_normal.z > 0) ? half_extents.z : -half_extents.z) +
 		   ofs;
 }
 
 Vector3 AABB::get_endpoint(int p_point) const {
-
 	switch (p_point) {
-		case 0: return Vector3(position.x, position.y, position.z);
-		case 1: return Vector3(position.x, position.y, position.z + size.z);
-		case 2: return Vector3(position.x, position.y + size.y, position.z);
-		case 3: return Vector3(position.x, position.y + size.y, position.z + size.z);
-		case 4: return Vector3(position.x + size.x, position.y, position.z);
-		case 5: return Vector3(position.x + size.x, position.y, position.z + size.z);
-		case 6: return Vector3(position.x + size.x, position.y + size.y, position.z);
-		case 7: return Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
-	};
+		case 0:
+			return Vector3(position.x, position.y, position.z);
+		case 1:
+			return Vector3(position.x, position.y, position.z + size.z);
+		case 2:
+			return Vector3(position.x, position.y + size.y, position.z);
+		case 3:
+			return Vector3(position.x, position.y + size.y, position.z + size.z);
+		case 4:
+			return Vector3(position.x + size.x, position.y, position.z);
+		case 5:
+			return Vector3(position.x + size.x, position.y, position.z + size.z);
+		case 6:
+			return Vector3(position.x + size.x, position.y + size.y, position.z);
+		case 7:
+			return Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
+	}
 
 	ERR_FAIL_V(Vector3());
 }
 
 bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const {
-
 	Vector3 half_extents = size * 0.5;
 	Vector3 ofs = position + half_extents;
 
@@ -202,8 +233,9 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con
 				(p.normal.y > 0) ? -half_extents.y : half_extents.y,
 				(p.normal.z > 0) ? -half_extents.z : half_extents.z);
 		point += ofs;
-		if (p.is_point_over(point))
+		if (p.is_point_over(point)) {
 			return false;
+		}
 	}
 
 	// Make sure all points in the shape aren't fully separated from the AABB on
@@ -212,7 +244,6 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con
 	int bad_point_counts_negative[3] = { 0 };
 
 	for (int k = 0; k < 3; k++) {
-
 		for (int i = 0; i < p_point_count; i++) {
 			if (p_points[i].coord[k] > ofs.coord[k] + half_extents.coord[k]) {
 				bad_point_counts_positive[k]++;
@@ -234,7 +265,6 @@ bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, con
 }
 
 bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const {
-
 	Vector3 half_extents = size * 0.5;
 	Vector3 ofs = position + half_extents;
 
@@ -245,56 +275,66 @@ bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const {
 				(p.normal.y < 0) ? -half_extents.y : half_extents.y,
 				(p.normal.z < 0) ? -half_extents.z : half_extents.z);
 		point += ofs;
-		if (p.is_point_over(point))
+		if (p.is_point_over(point)) {
 			return false;
+		}
 	}
 
 	return true;
 }
 
 bool AABB::has_point(const Vector3 &p_point) const {
-
-	if (p_point.x < position.x)
+	if (p_point.x < position.x) {
 		return false;
-	if (p_point.y < position.y)
+	}
+	if (p_point.y < position.y) {
 		return false;
-	if (p_point.z < position.z)
+	}
+	if (p_point.z < position.z) {
 		return false;
-	if (p_point.x > position.x + size.x)
+	}
+	if (p_point.x > position.x + size.x) {
 		return false;
-	if (p_point.y > position.y + size.y)
+	}
+	if (p_point.y > position.y + size.y) {
 		return false;
-	if (p_point.z > position.z + size.z)
+	}
+	if (p_point.z > position.z + size.z) {
 		return false;
+	}
 
 	return true;
 }
 
 inline void AABB::expand_to(const Vector3 &p_vector) {
-
 	Vector3 begin = position;
 	Vector3 end = position + size;
 
-	if (p_vector.x < begin.x)
+	if (p_vector.x < begin.x) {
 		begin.x = p_vector.x;
-	if (p_vector.y < begin.y)
+	}
+	if (p_vector.y < begin.y) {
 		begin.y = p_vector.y;
-	if (p_vector.z < begin.z)
+	}
+	if (p_vector.z < begin.z) {
 		begin.z = p_vector.z;
+	}
 
-	if (p_vector.x > end.x)
+	if (p_vector.x > end.x) {
 		end.x = p_vector.x;
-	if (p_vector.y > end.y)
+	}
+	if (p_vector.y > end.y) {
 		end.y = p_vector.y;
-	if (p_vector.z > end.z)
+	}
+	if (p_vector.z > end.z) {
 		end.z = p_vector.z;
+	}
 
 	position = begin;
 	size = end - begin;
 }
 
 void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const {
-
 	Vector3 half_extents(size.x * 0.5, size.y * 0.5, size.z * 0.5);
 	Vector3 center(position.x + half_extents.x, position.y + half_extents.y, position.z + half_extents.z);
 
@@ -305,7 +345,6 @@ void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r
 }
 
 inline real_t AABB::get_longest_axis_size() const {
-
 	real_t max_size = size.x;
 
 	if (size.y > max_size) {
@@ -320,7 +359,6 @@ inline real_t AABB::get_longest_axis_size() const {
 }
 
 inline real_t AABB::get_shortest_axis_size() const {
-
 	real_t max_size = size.x;
 
 	if (size.y < max_size) {
@@ -335,7 +373,6 @@ inline real_t AABB::get_shortest_axis_size() const {
 }
 
 bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const {
-
 	real_t divx = 1.0 / p_dir.x;
 	real_t divy = 1.0 / p_dir.y;
 	real_t divz = 1.0 / p_dir.z;
@@ -356,12 +393,15 @@ bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real
 		tymin = (upbound.y - p_from.y) * divy;
 		tymax = (position.y - p_from.y) * divy;
 	}
-	if ((tmin > tymax) || (tymin > tmax))
+	if ((tmin > tymax) || (tymin > tmax)) {
 		return false;
-	if (tymin > tmin)
+	}
+	if (tymin > tmin) {
 		tmin = tymin;
-	if (tymax < tmax)
+	}
+	if (tymax < tmax) {
 		tmax = tymax;
+	}
 	if (p_dir.z >= 0) {
 		tzmin = (position.z - p_from.z) * divz;
 		tzmax = (upbound.z - p_from.z) * divz;
@@ -369,17 +409,19 @@ bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real
 		tzmin = (upbound.z - p_from.z) * divz;
 		tzmax = (position.z - p_from.z) * divz;
 	}
-	if ((tmin > tzmax) || (tzmin > tmax))
+	if ((tmin > tzmax) || (tzmin > tmax)) {
 		return false;
-	if (tzmin > tmin)
+	}
+	if (tzmin > tmin) {
 		tmin = tzmin;
-	if (tzmax < tmax)
+	}
+	if (tzmax < tmax) {
 		tmax = tzmax;
+	}
 	return ((tmin < t1) && (tmax > t0));
 }
 
 void AABB::grow_by(real_t p_amount) {
-
 	position.x -= p_amount;
 	position.y -= p_amount;
 	position.z -= p_amount;
@@ -388,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