Fix PathFollow rotations.

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

4 files changed, 58 insertions, 37 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;
diff --git a/scene/3d/path.cpp b/scene/3d/path.cpp
index d535c545a8..5c29918118 100644
--- a/scene/3d/path.cpp
+++ b/scene/3d/path.cpp
@@ -108,40 +108,58 @@ void PathFollow::_update_transform() {
 	Vector3 pos = c->interpolate_baked(o, cubic);
 	Transform t = get_transform();
 
-	if (rotation_mode != ROTATION_NONE) {
-
-		Vector3 n = (c->interpolate_baked(o + lookahead, cubic) - pos).normalized();
-
-		if (rotation_mode == ROTATION_Y) {
+	t.origin = pos;
+	Vector3 pos_offset = Vector3(h_offset, v_offset, 0);
 
-			n.y = 0;
-			n.normalize();
-		}
+	if (rotation_mode != ROTATION_NONE) {
+		// perform parallel transport
+		//
+		// see C. Dougan, The Parallel Transport Frame, Game Programming Gems 2 for example
+		// for a discussion about why not Frenet frame.
+
+		Vector3 t_prev = pos - c->interpolate_baked(o - lookahead, cubic);
+		Vector3 t_cur = c->interpolate_baked(o + lookahead, cubic) - pos;
+
+		Vector3 axis = t_prev.cross(t_cur);
+		float dot = t_prev.normalized().dot(t_cur.normalized());
+		float angle = Math::acos(CLAMP(dot, -1, 1));
+
+		if (axis.length() > CMP_EPSILON && angle > CMP_EPSILON) {
+			if (rotation_mode == ROTATION_Y) {
+				// assuming we're referring to global Y-axis. is this correct?
+				axis.x = 0;
+				axis.z = 0;
+			} else if (rotation_mode == ROTATION_XY) {
+				axis.z = 0;
+			} else if (rotation_mode == ROTATION_XYZ) {
+				// all components are OK
+			}
 
-		if (n.length() < CMP_EPSILON) { //nothing, use previous
-			n = -t.get_basis().get_axis(2).normalized();
+			t.rotate_basis(axis.normalized(), angle);
 		}
 
-		Vector3 up = Vector3(0, 1, 0);
-
-		if (rotation_mode == ROTATION_XYZ) {
-
-			float tilt = c->interpolate_baked_tilt(o);
-			if (tilt != 0) {
-
-				Basis rot(-n, tilt); //remember.. lookat will be znegative.. znegative!! we abide by opengl clan.
-				up = rot.xform(up);
+		// do the additional tilting
+		float tilt_angle = c->interpolate_baked_tilt(o);
+		Vector3 tilt_axis = t_cur; // is this correct??
+
+		if (tilt_axis.length() > CMP_EPSILON && tilt_angle > CMP_EPSILON) {
+			if (rotation_mode == ROTATION_Y) {
+				tilt_axis.x = 0;
+				tilt_axis.z = 0;
+			} else if (rotation_mode == ROTATION_XY) {
+				tilt_axis.z = 0;
+			} else if (rotation_mode == ROTATION_XYZ) {
+				// all components are OK
 			}
-		}
 
-		t.set_look_at(pos, pos + n, up);
+			t.rotate_basis(tilt_axis.normalized(), tilt_angle);
+		}
 
+		t.translate(pos_offset);
 	} else {
-
-		t.origin = pos;
+		t.origin += pos_offset;
 	}
 
-	t.origin += t.basis.get_axis(0) * h_offset + t.basis.get_axis(1) * v_offset;
 	set_transform(t);
 }