Merge pull request godotengine#6 from lawnjelly/robust_normalize_3

Robust normalize(), normalized() and normalize_and_get_length()

core/math/math_defs.h

@@ -34,6 +34,11 @@
 #define CMP_EPSILON 0.00001
 #define CMP_EPSILON2 (CMP_EPSILON * CMP_EPSILON)
 
+// These two are calculated empirically from 32bit SSE on x86
+#define NORMALIZE_DIRECTION_EPSILON_SQRT (1e-10f)
+#define NORMALIZE_DIRECTION_EPSILON_SQRT_SQUARED (NORMALIZE_DIRECTION_EPSILON_SQRT * NORMALIZE_DIRECTION_EPSILON_SQRT)
+#define NORMALIZE_DIRECTION_EPSILON (FLT_MIN > NORMALIZE_DIRECTION_EPSILON_SQRT_SQUARED ? FLT_MIN : NORMALIZE_DIRECTION_EPSILON_SQRT_SQUARED)
+
 #define CMP_NORMALIZE_TOLERANCE 0.000001
 #define CMP_POINT_IN_PLANE_EPSILON 0.00001
 

core/math/vector2.cpp

@@ -42,19 +42,12 @@ real_t Vector2::length_squared() const {
 	return x * x + y * y;
 }
 
-void Vector2::normalize() {
-	real_t l = x * x + y * y;
-	if (l != 0) {
-		l = Math::sqrt(l);
-		x /= l;
-		y /= l;
-	}
-}
-
 Vector2 Vector2::normalized() const {
 	Vector2 v = *this;
-	v.normalize();
-	return v;
+	if (v.normalize_and_get_length()) {
+		return v;
+	}
+	return Vector2();
 }
 
 bool Vector2::is_normalized() const {

core/math/vector2.h

@@ -80,7 +80,8 @@ struct _NO_DISCARD_CLASS_ Vector2 {
 		return x < y ? 1 : 0;
 	}
 
-	void normalize();
+	bool normalize() { return normalize_and_get_length(); }
+	inline bool normalize_and_get_length(real_t *r_length = nullptr);
 	Vector2 normalized() const;
 	bool is_normalized() const;
 
@@ -243,6 +244,43 @@ Vector2 Vector2::linear_interpolate(const Vector2 &p_to, real_t p_weight) const
 	return res;
 }
 
+bool Vector2::normalize_and_get_length(real_t *r_length) {
+	real_t lengthsq = length_squared();
+
+	// Long enough to guarantee unit length result.
+	if (lengthsq >= (real_t)CMP_EPSILON2) {
+		real_t length = Math::sqrt(lengthsq);
+		*this /= length;
+		if (r_length) {
+			*r_length = length;
+		}
+		return true;
+	}
+	// Long enough to guarantee direction, but not unit length result.
+	constexpr real_t epsilon = NORMALIZE_DIRECTION_EPSILON;
+	if (lengthsq >= epsilon) {
+		// Boost length prior to normalize
+		// to guarantee unit length.
+		constexpr real_t mult = (1.0 / NORMALIZE_DIRECTION_EPSILON_SQRT);
+		*this *= mult;
+
+		lengthsq = length_squared();
+
+		real_t length = Math::sqrt(lengthsq);
+		*this /= length;
+
+		// The length has been boosted so we need to un-boost to get the original length.
+		if (r_length) {
+			*r_length = length * NORMALIZE_DIRECTION_EPSILON_SQRT;
+		}
+		return true;
+	}
+	// Not long enough to guarantee direction,
+	// failed.
+
+	return false;
+}
+
 Vector2 Vector2::slerp(const Vector2 &p_to, real_t p_weight) const {
 #ifdef MATH_CHECKS
 	ERR_FAIL_COND_V_MSG(!is_normalized(), Vector2(), "The start Vector2 must be normalized.");

core/math/vector3.h

@@ -83,7 +83,8 @@ struct _NO_DISCARD_CLASS_ Vector3 {
 	_FORCE_INLINE_ real_t length() const;
 	_FORCE_INLINE_ real_t length_squared() const;
 
-	_FORCE_INLINE_ void normalize();
+	bool normalize() { return normalize_and_get_length(); }
+	inline bool normalize_and_get_length(real_t *r_length = nullptr);
 	_FORCE_INLINE_ Vector3 normalized() const;
 	_FORCE_INLINE_ bool is_normalized() const;
 	_FORCE_INLINE_ Vector3 inverse() const;
@@ -405,22 +406,49 @@ real_t Vector3::length_squared() const {
 	return x2 + y2 + z2;
 }
 
-void Vector3::normalize() {
+bool Vector3::normalize_and_get_length(real_t *r_length) {
 	real_t lengthsq = length_squared();
-	if (lengthsq == 0) {
-		x = y = z = 0;
-	} else {
+
+	// Long enough to guarantee unit length result.
+	if (lengthsq >= (real_t)CMP_EPSILON2) {
 		real_t length = Math::sqrt(lengthsq);
-		x /= length;
-		y /= length;
-		z /= length;
+		*this /= length;
+		if (r_length) {
+			*r_length = length;
+		}
+		return true;
 	}
+	// Long enough to guarantee direction, but not unit length result.
+	constexpr real_t epsilon = NORMALIZE_DIRECTION_EPSILON;
+	if (lengthsq >= epsilon) {
+		// Boost length prior to normalize
+		// to guarantee unit length.
+		constexpr real_t mult = (1.0 / NORMALIZE_DIRECTION_EPSILON_SQRT);
+		*this *= mult;
+
+		lengthsq = length_squared();
+
+		real_t length = Math::sqrt(lengthsq);
+		*this /= length;
+
+		// The length has been boosted so we need to un-boost to get the original length.
+		if (r_length) {
+			*r_length = length * NORMALIZE_DIRECTION_EPSILON_SQRT;
+		}
+		return true;
+	}
+	// Not long enough to guarantee direction,
+	// failed.
+
+	return false;
 }
 
 Vector3 Vector3::normalized() const {
 	Vector3 v = *this;
-	v.normalize();
-	return v;
+	if (v.normalize_and_get_length()) {
+		return v;
+	}
+	return Vector3();
 }
 
 bool Vector3::is_normalized() const {