4 files changed, 29 insertions, 17 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 4f80fb2491..ffe1089965 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -443,4 +443,5 @@ AStar::AStar() {
 AStar::~AStar() {
 
 	pass = 1;
+	clear();
 }
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index d63da322a5..65b2ffb0df 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -387,6 +387,23 @@ public:
 
 		return hf;
 	}
+
+	static _ALWAYS_INLINE_ float snap_scalar(float p_offset, float p_step, float p_target) {
+		return p_step != 0 ? Math::stepify(p_target - p_offset, p_step) + p_offset : p_target;
+	}
+
+	static _ALWAYS_INLINE_ float snap_scalar_seperation(float p_offset, float p_step, float p_target, float p_separation) {
+		if (p_step != 0) {
+			float a = Math::stepify(p_target - p_offset, p_step + p_separation) + p_offset;
+			float b = a;
+			if (p_target >= 0)
+				b -= p_separation;
+			else
+				b += p_step;
+			return (Math::abs(p_target - a) < Math::abs(p_target - b)) ? a : b;
+		}
+		return p_target;
+	}
 };
 
 #endif // MATH_FUNCS_H
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index 4051de7afb..85421c074b 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -538,7 +538,7 @@ Basis::operator String() const {
 	return mtx;
 }
 
-Basis::operator Quat() const {
+Quat Basis::get_quat() const {
 	//commenting this check because precision issues cause it to fail when it shouldn't
 	//#ifdef MATH_CHECKS
 	//ERR_FAIL_COND_V(is_rotation() == false, Quat());
@@ -710,12 +710,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 	r_angle = angle;
 }
 
-Basis::Basis(const Vector3 &p_euler) {
-
-	set_euler(p_euler);
-}
-
-Basis::Basis(const Quat &p_quat) {
+void Basis::set_quat(const Quat &p_quat) {
 
 	real_t d = p_quat.length_squared();
 	real_t s = 2.0 / d;
@@ -750,7 +745,3 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
 	elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine;
 	elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z);
 }
-
-Basis::Basis(const Vector3 &p_axis, real_t p_phi) {
-	set_axis_angle(p_axis, p_phi);
-}
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 23429888e0..9a33b8203d 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -88,8 +88,11 @@ public:
 	Vector3 get_euler_yxz() const;
 	void set_euler_yxz(const Vector3 &p_euler);
 
-	Vector3 get_euler() const { return get_euler_yxz(); };
-	void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); };
+	Quat get_quat() const;
+	void set_quat(const Quat &p_quat);
+
+	Vector3 get_euler() const { return get_euler_yxz(); }
+	void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }
 
 	void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
 	void set_axis_angle(const Vector3 &p_axis, real_t p_phi);
@@ -205,11 +208,11 @@ public:
 	bool is_symmetric() const;
 	Basis diagonalize();
 
-	operator Quat() const;
+	operator Quat() const { return get_quat(); }
 
-	Basis(const Quat &p_quat); // euler
-	Basis(const Vector3 &p_euler); // euler
-	Basis(const Vector3 &p_axis, real_t p_phi);
+	Basis(const Quat &p_quat) { set_quat(p_quat); };
+	Basis(const Vector3 &p_euler) { set_euler(p_euler); }
+	Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); }
 
 	_FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) {
 		elements[0] = row0;