Implement snappedi, snappedf, and Vector[2/3/4]i.snapped

2 files changed, 87 insertions, 3 deletions

diff --git a/core/variant/variant_call.cpp b/core/variant/variant_call.cpp
index 91af2bab85..721cf71808 100644
--- a/core/variant/variant_call.cpp
+++ b/core/variant/variant_call.cpp
@@ -1646,6 +1646,7 @@ static void _register_variant_builtin_methods() {
 	bind_method(Vector2i, sign, sarray(), varray());
 	bind_method(Vector2i, abs, sarray(), varray());
 	bind_method(Vector2i, clamp, sarray("min", "max"), varray());
+	bind_method(Vector2i, snapped, sarray("step"), varray());
 
 	/* Rect2 */
 
@@ -1734,6 +1735,7 @@ static void _register_variant_builtin_methods() {
 	bind_method(Vector3i, sign, sarray(), varray());
 	bind_method(Vector3i, abs, sarray(), varray());
 	bind_method(Vector3i, clamp, sarray("min", "max"), varray());
+	bind_method(Vector3i, snapped, sarray("step"), varray());
 
 	/* Vector4 */
 
@@ -1773,6 +1775,7 @@ static void _register_variant_builtin_methods() {
 	bind_method(Vector4i, sign, sarray(), varray());
 	bind_method(Vector4i, abs, sarray(), varray());
 	bind_method(Vector4i, clamp, sarray("min", "max"), varray());
+	bind_method(Vector4i, snapped, sarray("step"), varray());
 
 	/* Plane */
 
diff --git a/core/variant/variant_utility.cpp b/core/variant/variant_utility.cpp
index 3843c32bcc..e8883f96aa 100644
--- a/core/variant/variant_utility.cpp
+++ b/core/variant/variant_utility.cpp
@@ -322,8 +322,52 @@ struct VariantUtilityFunctions {
 		return Math::step_decimals(step);
 	}
 
-	static inline double snapped(double value, double step) {
-		return Math::snapped(value, step);
+	static inline Variant snapped(const Variant &x, const Variant &step, Callable::CallError &r_error) {
+		r_error.error = Callable::CallError::CALL_OK;
+		if (x.get_type() != step.get_type() && !((x.get_type() == Variant::INT && step.get_type() == Variant::FLOAT) || (x.get_type() == Variant::FLOAT && step.get_type() == Variant::INT))) {
+			r_error.error = Callable::CallError::CALL_ERROR_INVALID_ARGUMENT;
+			r_error.argument = 1;
+			return Variant();
+		}
+
+		switch (step.get_type()) {
+			case Variant::INT: {
+				return snappedi(x, VariantInternalAccessor<int64_t>::get(&step));
+			} break;
+			case Variant::FLOAT: {
+				return snappedf(x, VariantInternalAccessor<double>::get(&step));
+			} break;
+			case Variant::VECTOR2: {
+				return VariantInternalAccessor<Vector2>::get(&x).snapped(VariantInternalAccessor<Vector2>::get(&step));
+			} break;
+			case Variant::VECTOR2I: {
+				return VariantInternalAccessor<Vector2i>::get(&x).snapped(VariantInternalAccessor<Vector2i>::get(&step));
+			} break;
+			case Variant::VECTOR3: {
+				return VariantInternalAccessor<Vector3>::get(&x).snapped(VariantInternalAccessor<Vector3>::get(&step));
+			} break;
+			case Variant::VECTOR3I: {
+				return VariantInternalAccessor<Vector3i>::get(&x).snapped(VariantInternalAccessor<Vector3i>::get(&step));
+			} break;
+			case Variant::VECTOR4: {
+				return VariantInternalAccessor<Vector4>::get(&x).snapped(VariantInternalAccessor<Vector4>::get(&step));
+			} break;
+			case Variant::VECTOR4I: {
+				return VariantInternalAccessor<Vector4i>::get(&x).snapped(VariantInternalAccessor<Vector4i>::get(&step));
+			} break;
+			default: {
+				r_error.error = Callable::CallError::CALL_ERROR_INVALID_METHOD;
+				return Variant();
+			}
+		}
+	}
+
+	static inline double snappedf(double x, double step) {
+		return Math::snapped(x, step);
+	}
+
+	static inline int64_t snappedi(double x, int64_t step) {
+		return Math::snapped(x, step);
 	}
 
 	static inline Variant lerp(const Variant &from, const Variant &to, double weight, Callable::CallError &r_error) {
@@ -1132,6 +1176,40 @@ static _FORCE_INLINE_ Variant::Type get_ret_type_helper(void (*p_func)(P...)) {
 	};                                                                                                                  \
 	register_utility_function<Func_##m_func>(#m_func, m_args)
 
+#define FUNCBINDVR2(m_func, m_args, m_category)                                                                                    \
+	class Func_##m_func {                                                                                                          \
+	public:                                                                                                                        \
+		static void call(Variant *r_ret, const Variant **p_args, int p_argcount, Callable::CallError &r_error) {                   \
+			r_error.error = Callable::CallError::CALL_OK;                                                                          \
+			*r_ret = VariantUtilityFunctions::m_func(*p_args[0], *p_args[1], r_error);                                             \
+		}                                                                                                                          \
+		static void validated_call(Variant *r_ret, const Variant **p_args, int p_argcount) {                                       \
+			Callable::CallError ce;                                                                                                \
+			*r_ret = VariantUtilityFunctions::m_func(*p_args[0], *p_args[1], ce);                                                  \
+		}                                                                                                                          \
+		static void ptrcall(void *ret, const void **p_args, int p_argcount) {                                                      \
+			Callable::CallError ce;                                                                                                \
+			Variant r;                                                                                                             \
+			r = VariantUtilityFunctions::m_func(PtrToArg<Variant>::convert(p_args[0]), PtrToArg<Variant>::convert(p_args[1]), ce); \
+			PtrToArg<Variant>::encode(r, ret);                                                                                     \
+		}                                                                                                                          \
+		static int get_argument_count() {                                                                                          \
+			return 2;                                                                                                              \
+		}                                                                                                                          \
+		static Variant::Type get_argument_type(int p_arg) {                                                                        \
+			return Variant::NIL;                                                                                                   \
+		}                                                                                                                          \
+		static Variant::Type get_return_type() {                                                                                   \
+			return Variant::NIL;                                                                                                   \
+		}                                                                                                                          \
+		static bool has_return_type() {                                                                                            \
+			return true;                                                                                                           \
+		}                                                                                                                          \
+		static bool is_vararg() { return false; }                                                                                  \
+		static Variant::UtilityFunctionType get_type() { return m_category; }                                                      \
+	};                                                                                                                             \
+	register_utility_function<Func_##m_func>(#m_func, m_args)
+
 #define FUNCBINDVR3(m_func, m_args, m_category)                                                                                                                           \
 	class Func_##m_func {                                                                                                                                                 \
 	public:                                                                                                                                                               \
@@ -1415,6 +1493,10 @@ void Variant::_register_variant_utility_functions() {
 	FUNCBINDR(signf, sarray("x"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(signi, sarray("x"), Variant::UTILITY_FUNC_TYPE_MATH);
 
+	FUNCBINDVR2(snapped, sarray("x", "step"), Variant::UTILITY_FUNC_TYPE_MATH);
+	FUNCBINDR(snappedf, sarray("x", "step"), Variant::UTILITY_FUNC_TYPE_MATH);
+	FUNCBINDR(snappedi, sarray("x", "step"), Variant::UTILITY_FUNC_TYPE_MATH);
+
 	FUNCBINDR(pow, sarray("base", "exp"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(log, sarray("x"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(exp, sarray("x"), Variant::UTILITY_FUNC_TYPE_MATH);
@@ -1428,7 +1510,6 @@ void Variant::_register_variant_utility_functions() {
 
 	FUNCBINDR(ease, sarray("x", "curve"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(step_decimals, sarray("x"), Variant::UTILITY_FUNC_TYPE_MATH);
-	FUNCBINDR(snapped, sarray("x", "step"), Variant::UTILITY_FUNC_TYPE_MATH);
 
 	FUNCBINDVR3(lerp, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(lerpf, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);