Fix PathFollow rotations.

Used parallel transport to move the object along the curve. Also introduced a few more math checks useful for debugging.

3 files changed, 16 insertions, 13 deletions

diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index c3eeb1f705..8897c692f7 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -145,6 +145,12 @@ public:
 		elements[2][1] = zy;
 		elements[2][2] = zz;
 	}
+	_FORCE_INLINE_ void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
+
+		set_axis(0, p_x);
+		set_axis(1, p_y);
+		set_axis(2, p_z);
+	}
 	_FORCE_INLINE_ Vector3 get_column(int i) const {
 
 		return Vector3(elements[0][i], elements[1][i], elements[2][i]);
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
index e53e6cf519..7a50214596 100644
--- a/core/math/transform.cpp
+++ b/core/math/transform.cpp
@@ -82,7 +82,10 @@ Transform Transform::looking_at(const Vector3 &p_target, const Vector3 &p_up) co
 }
 
 void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) {
-
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND(p_eye == p_target);
+	ERR_FAIL_COND(p_up.length() == 0);
+#endif
 	// Reference: MESA source code
 	Vector3 v_x, v_y, v_z;
 
@@ -96,6 +99,9 @@ void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const
 	v_y = p_up;
 
 	v_x = v_y.cross(v_z);
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND(v_x.length() == 0);
+#endif
 
 	/* Recompute Y = Z cross X */
 	v_y = v_z.cross(v_x);
@@ -103,9 +109,8 @@ void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const
 	v_x.normalize();
 	v_y.normalize();
 
-	basis.set_axis(0, v_x);
-	basis.set_axis(1, v_y);
-	basis.set_axis(2, v_z);
+	basis.set(v_x, v_y, v_z);
+
 	origin = p_eye;
 }
 
diff --git a/core/math/transform.h b/core/math/transform.h
index 4731496bf3..bbb25a420a 100644
--- a/core/math/transform.h
+++ b/core/math/transform.h
@@ -97,15 +97,7 @@ public:
 
 	void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t tx, real_t ty, real_t tz) {
 
-		basis.elements[0][0] = xx;
-		basis.elements[0][1] = xy;
-		basis.elements[0][2] = xz;
-		basis.elements[1][0] = yx;
-		basis.elements[1][1] = yy;
-		basis.elements[1][2] = yz;
-		basis.elements[2][0] = zx;
-		basis.elements[2][1] = zy;
-		basis.elements[2][2] = zz;
+		basis.set(xx, xy, xz, yx, yy, yz, zx, zy, zz);
 		origin.x = tx;
 		origin.y = ty;
 		origin.z = tz;