Merge pull request #18992 from aaronfranke/mono-equal-approx

[Core] [Mono] Improve and use approximate equality methods

8 files changed, 41 insertions, 33 deletions

diff --git a/core/math/delaunay.h b/core/math/delaunay.h
index bd0cf97937..ed52c506db 100644
--- a/core/math/delaunay.h
+++ b/core/math/delaunay.h
@@ -80,11 +80,11 @@ public:
 	}
 
 	static bool edge_compare(const Vector<Vector2> &p_vertices, const Edge &p_a, const Edge &p_b) {
-		if (p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[0]]) < CMP_EPSILON && p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[1]]) < CMP_EPSILON) {
+		if (Math::is_zero_approx(p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[0]])) && Math::is_zero_approx(p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[1]]))) {
 			return true;
 		}
 
-		if (p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[1]]) < CMP_EPSILON && p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[0]]) < CMP_EPSILON) {
+		if (Math::is_zero_approx(p_vertices[p_a.edge[0]].distance_to(p_vertices[p_b.edge[1]])) && Math::is_zero_approx(p_vertices[p_a.edge[1]].distance_to(p_vertices[p_b.edge[0]]))) {
 			return true;
 		}
 
diff --git a/core/math/geometry.cpp b/core/math/geometry.cpp
index a84b5a16c7..0ab8707d3a 100644
--- a/core/math/geometry.cpp
+++ b/core/math/geometry.cpp
@@ -836,7 +836,7 @@ Geometry::MeshData Geometry::build_convex_mesh(const PoolVector<Plane> &p_planes
 					Vector3 rel = edge1_A - edge0_A;
 
 					real_t den = clip.normal.dot(rel);
-					if (Math::abs(den) < CMP_EPSILON)
+					if (Math::is_zero_approx(den))
 						continue; // point too short
 
 					real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den;
diff --git a/core/math/geometry.h b/core/math/geometry.h
index 7347cb742a..f3a671aa9a 100644
--- a/core/math/geometry.h
+++ b/core/math/geometry.h
@@ -181,8 +181,8 @@ public:
 			}
 		}
 		// finally do the division to get sc and tc
-		sc = (Math::abs(sN) < CMP_EPSILON ? 0.0 : sN / sD);
-		tc = (Math::abs(tN) < CMP_EPSILON ? 0.0 : tN / tD);
+		sc = (Math::is_zero_approx(sN) ? 0.0 : sN / sD);
+		tc = (Math::is_zero_approx(tN) ? 0.0 : tN / tD);
 
 		// get the difference of the two closest points
 		Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
@@ -195,7 +195,7 @@ public:
 		Vector3 e2 = p_v2 - p_v0;
 		Vector3 h = p_dir.cross(e2);
 		real_t a = e1.dot(h);
-		if (a > -CMP_EPSILON && a < CMP_EPSILON) // parallel test
+		if (Math::is_zero_approx(a)) // parallel test
 			return false;
 
 		real_t f = 1.0 / a;
@@ -233,7 +233,7 @@ public:
 		Vector3 e2 = p_v2 - p_v0;
 		Vector3 h = rel.cross(e2);
 		real_t a = e1.dot(h);
-		if (a > -CMP_EPSILON && a < CMP_EPSILON) // parallel test
+		if (Math::is_zero_approx(a)) // parallel test
 			return false;
 
 		real_t f = 1.0 / a;
@@ -535,7 +535,7 @@ public:
 		// see http://paulbourke.net/geometry/pointlineplane/
 
 		const real_t denom = p_dir_b.y * p_dir_a.x - p_dir_b.x * p_dir_a.y;
-		if (Math::abs(denom) < CMP_EPSILON) { // parallel?
+		if (Math::is_zero_approx(denom)) { // parallel?
 			return false;
 		}
 
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 0d209402dd..a75f2fb4ab 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -272,13 +272,20 @@ public:
 		return diff < epsilon;
 	}
 
-	static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b, real_t epsilon = CMP_EPSILON) {
-		// TODO: Comparing floats for approximate-equality is non-trivial.
-		// Using epsilon should cover the typical cases in Godot (where a == b is used to compare two reals), such as matrix and vector comparison operators.
-		// A proper implementation in terms of ULPs should eventually replace the contents of this function.
-		// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ for details.
+	static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b) {
+		real_t tolerance = CMP_EPSILON * abs(a);
+		if (tolerance < CMP_EPSILON) {
+			tolerance = CMP_EPSILON;
+		}
+		return abs(a - b) < tolerance;
+	}
+
+	static _ALWAYS_INLINE_ bool is_equal_approx(real_t a, real_t b, real_t tolerance) {
+		return abs(a - b) < tolerance;
+	}
 
-		return abs(a - b) < epsilon;
+	static _ALWAYS_INLINE_ bool is_zero_approx(real_t s) {
+		return abs(s) < CMP_EPSILON;
 	}
 
 	static _ALWAYS_INLINE_ float absf(float g) {
diff --git a/core/math/plane.cpp b/core/math/plane.cpp
index cd3cbce300..b01853c4ac 100644
--- a/core/math/plane.cpp
+++ b/core/math/plane.cpp
@@ -110,7 +110,7 @@ bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3
 	real_t den = normal.dot(segment);
 
 	//printf("den is %i\n",den);
-	if (Math::abs(den) <= CMP_EPSILON) {
+	if (Math::is_zero_approx(den)) {
 
 		return false;
 	}
@@ -135,7 +135,7 @@ bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vec
 	real_t den = normal.dot(segment);
 
 	//printf("den is %i\n",den);
-	if (Math::abs(den) <= CMP_EPSILON) {
+	if (Math::is_zero_approx(den)) {
 
 		return false;
 	}
diff --git a/core/math/plane.h b/core/math/plane.h
index 1c6e4b816b..ec817edd2c 100644
--- a/core/math/plane.h
+++ b/core/math/plane.h
@@ -125,12 +125,12 @@ Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_
 
 bool Plane::operator==(const Plane &p_plane) const {
 
-	return normal == p_plane.normal && d == p_plane.d;
+	return normal == p_plane.normal && Math::is_equal_approx(d, p_plane.d);
 }
 
 bool Plane::operator!=(const Plane &p_plane) const {
 
-	return normal != p_plane.normal || d != p_plane.d;
+	return normal != p_plane.normal || !Math::is_equal_approx(d, p_plane.d);
 }
 
 #endif // PLANE_H
diff --git a/core/math/vector2.h b/core/math/vector2.h
index 9a214ef9b5..a0c6024c9f 100644
--- a/core/math/vector2.h
+++ b/core/math/vector2.h
@@ -106,8 +106,8 @@ struct Vector2 {
 	bool operator==(const Vector2 &p_vec2) const;
 	bool operator!=(const Vector2 &p_vec2) const;
 
-	bool operator<(const Vector2 &p_vec2) const { return (x == p_vec2.x) ? (y < p_vec2.y) : (x < p_vec2.x); }
-	bool operator<=(const Vector2 &p_vec2) const { return (x == p_vec2.x) ? (y <= p_vec2.y) : (x <= p_vec2.x); }
+	bool operator<(const Vector2 &p_vec2) const { return (Math::is_equal_approx(x, p_vec2.x)) ? (y < p_vec2.y) : (x < p_vec2.x); }
+	bool operator<=(const Vector2 &p_vec2) const { return (Math::is_equal_approx(x, p_vec2.x)) ? (y <= p_vec2.y) : (x < p_vec2.x); }
 
 	real_t angle() const;
 
@@ -213,11 +213,11 @@ _FORCE_INLINE_ Vector2 Vector2::operator-() const {
 
 _FORCE_INLINE_ bool Vector2::operator==(const Vector2 &p_vec2) const {
 
-	return x == p_vec2.x && y == p_vec2.y;
+	return Math::is_equal_approx(x, p_vec2.x) && Math::is_equal_approx(y, p_vec2.y);
 }
 _FORCE_INLINE_ bool Vector2::operator!=(const Vector2 &p_vec2) const {
 
-	return x != p_vec2.x || y != p_vec2.y;
+	return !Math::is_equal_approx(x, p_vec2.x) || !Math::is_equal_approx(y, p_vec2.y);
 }
 
 Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const {
diff --git a/core/math/vector3.h b/core/math/vector3.h
index e9074c5bd4..21fc09653f 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -341,17 +341,17 @@ Vector3 Vector3::operator-() const {
 
 bool Vector3::operator==(const Vector3 &p_v) const {
 
-	return (x == p_v.x && y == p_v.y && z == p_v.z);
+	return (Math::is_equal_approx(x, p_v.x) && Math::is_equal_approx(y, p_v.y) && Math::is_equal_approx(z, p_v.z));
 }
 
 bool Vector3::operator!=(const Vector3 &p_v) const {
-	return (x != p_v.x || y != p_v.y || z != p_v.z);
+	return (!Math::is_equal_approx(x, p_v.x) || !Math::is_equal_approx(y, p_v.y) || !Math::is_equal_approx(z, p_v.z));
 }
 
 bool Vector3::operator<(const Vector3 &p_v) const {
 
-	if (x == p_v.x) {
-		if (y == p_v.y)
+	if (Math::is_equal_approx(x, p_v.x)) {
+		if (Math::is_equal_approx(y, p_v.y))
 			return z < p_v.z;
 		else
 			return y < p_v.y;
@@ -362,8 +362,8 @@ bool Vector3::operator<(const Vector3 &p_v) const {
 
 bool Vector3::operator<=(const Vector3 &p_v) const {
 
-	if (x == p_v.x) {
-		if (y == p_v.y)
+	if (Math::is_equal_approx(x, p_v.x)) {
+		if (Math::is_equal_approx(y, p_v.y))
 			return z <= p_v.z;
 		else
 			return y < p_v.y;
@@ -402,13 +402,14 @@ real_t Vector3::length_squared() const {
 
 void Vector3::normalize() {
 
-	real_t l = length();
-	if (l == 0) {
+	real_t lengthsq = length_squared();
+	if (lengthsq == 0) {
 		x = y = z = 0;
 	} else {
-		x /= l;
-		y /= l;
-		z /= l;
+		real_t length = Math::sqrt(lengthsq);
+		x /= length;
+		y /= length;
+		z /= length;
 	}
 }