4 files changed, 43 insertions, 7 deletions

diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index 38dad893f5..676a0004ea 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -210,6 +210,14 @@ Vector2i Vector2i::clamp(const Vector2i &p_min, const Vector2i &p_max) const {
 			CLAMP(y, p_min.y, p_max.y));
 }
 
+int64_t Vector2i::length_squared() const {
+	return x * (int64_t)x + y * (int64_t)y;
+}
+
+double Vector2i::length() const {
+	return Math::sqrt((double)length_squared());
+}
+
 Vector2i Vector2i::operator+(const Vector2i &p_v) const {
 	return Vector2i(x + p_v.x, y + p_v.y);
 }
diff --git a/core/math/vector2.h b/core/math/vector2.h
index 493e0af27d..a340036ac7 100644
--- a/core/math/vector2.h
+++ b/core/math/vector2.h
@@ -261,11 +261,16 @@ Vector2 Vector2::lerp(const Vector2 &p_to, const real_t p_weight) const {
 }
 
 Vector2 Vector2::slerp(const Vector2 &p_to, const real_t p_weight) const {
-#ifdef MATH_CHECKS
-	ERR_FAIL_COND_V_MSG(!is_normalized(), Vector2(), "The start Vector2 must be normalized.");
-#endif
-	real_t theta = angle_to(p_to);
-	return rotated(theta * p_weight);
+	real_t start_length_sq = length_squared();
+	real_t end_length_sq = p_to.length_squared();
+	if (unlikely(start_length_sq == 0.0 || end_length_sq == 0.0)) {
+		// Zero length vectors have no angle, so the best we can do is either lerp or throw an error.
+		return lerp(p_to, p_weight);
+	}
+	real_t start_length = Math::sqrt(start_length_sq);
+	real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight);
+	real_t angle = angle_to(p_to);
+	return rotated(angle * p_weight) * (result_length / start_length);
 }
 
 Vector2 Vector2::direction_to(const Vector2 &p_to) const {
@@ -344,6 +349,9 @@ struct Vector2i {
 	bool operator==(const Vector2i &p_vec2) const;
 	bool operator!=(const Vector2i &p_vec2) const;
 
+	int64_t length_squared() const;
+	double length() const;
+
 	real_t aspect() const { return width / (real_t)height; }
 	Vector2i sign() const { return Vector2i(SIGN(x), SIGN(y)); }
 	Vector2i abs() const { return Vector2i(ABS(x), ABS(y)); }
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 1861627718..d7a72b05a8 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -240,8 +240,16 @@ Vector3 Vector3::lerp(const Vector3 &p_to, const real_t p_weight) const {
 }
 
 Vector3 Vector3::slerp(const Vector3 &p_to, const real_t p_weight) const {
-	real_t theta = angle_to(p_to);
-	return rotated(cross(p_to).normalized(), theta * p_weight);
+	real_t start_length_sq = length_squared();
+	real_t end_length_sq = p_to.length_squared();
+	if (unlikely(start_length_sq == 0.0 || end_length_sq == 0.0)) {
+		// Zero length vectors have no angle, so the best we can do is either lerp or throw an error.
+		return lerp(p_to, p_weight);
+	}
+	real_t start_length = Math::sqrt(start_length_sq);
+	real_t result_length = Math::lerp(start_length, Math::sqrt(end_length_sq), p_weight);
+	real_t angle = angle_to(p_to);
+	return rotated(cross(p_to).normalized(), angle * p_weight) * (result_length / start_length);
 }
 
 real_t Vector3::distance_to(const Vector3 &p_to) const {
diff --git a/core/math/vector3i.h b/core/math/vector3i.h
index 0f9caa349b..1416c98057 100644
--- a/core/math/vector3i.h
+++ b/core/math/vector3i.h
@@ -31,6 +31,7 @@
 #ifndef VECTOR3I_H
 #define VECTOR3I_H
 
+#include "core/math/math_funcs.h"
 #include "core/string/ustring.h"
 #include "core/typedefs.h"
 
@@ -65,6 +66,9 @@ struct Vector3i {
 	Vector3i::Axis min_axis_index() const;
 	Vector3i::Axis max_axis_index() const;
 
+	_FORCE_INLINE_ int64_t length_squared() const;
+	_FORCE_INLINE_ double length() const;
+
 	_FORCE_INLINE_ void zero();
 
 	_FORCE_INLINE_ Vector3i abs() const;
@@ -110,6 +114,14 @@ struct Vector3i {
 	}
 };
 
+int64_t Vector3i::length_squared() const {
+	return x * (int64_t)x + y * (int64_t)y + z * (int64_t)z;
+}
+
+double Vector3i::length() const {
+	return Math::sqrt((double)length_squared());
+}
+
 Vector3i Vector3i::abs() const {
 	return Vector3i(ABS(x), ABS(y), ABS(z));
 }