6 files changed, 34 insertions, 26 deletions

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;
 	}
 }
 
diff --git a/scene/resources/animation.cpp b/scene/resources/animation.cpp
index e58ec9d71e..1caa24d411 100644
--- a/scene/resources/animation.cpp
+++ b/scene/resources/animation.cpp
@@ -1477,7 +1477,7 @@ int Animation::_find(const Vector<K> &p_keys, float p_time) const {
 
 		middle = (low + high) / 2;
 
-		if (Math::abs(p_time - keys[middle].time) < CMP_EPSILON) { //match
+		if (Math::is_equal_approx(p_time, keys[middle].time)) { //match
 			return middle;
 		} else if (p_time < keys[middle].time)
 			high = middle - 1; //search low end of array