BGE: slow parent was causing scaling distortion, now use correct quaternion interpolation.

2 files changed, 22 insertions, 25 deletions

diff --git a/intern/moto/include/MT_Quaternion.inl b/intern/moto/include/MT_Quaternion.inl
index ecfd6699f67..ec747c453d3 100644
--- a/intern/moto/include/MT_Quaternion.inl
+++ b/intern/moto/include/MT_Quaternion.inl
@@ -74,19 +74,27 @@ GEN_INLINE MT_Scalar MT_Quaternion::angle(const MT_Quaternion& q) const
 
 GEN_INLINE MT_Quaternion MT_Quaternion::slerp(const MT_Quaternion& q, const MT_Scalar& t) const
 {
-	MT_Scalar theta = angle(q);
-	
-	if (!MT_fuzzyZero(theta))
+	MT_Scalar d, s0, s1;
+	MT_Scalar s = dot(q);
+	bool neg = (s < 0.0);
+
+	if (neg)
+		s = -s;
+	if ((1.0 - s) > 0.0001) 
 	{
-		MT_Scalar d = MT_Scalar(1.0) / sin(theta);
-		MT_Scalar s0 = sin((MT_Scalar(1.0) - t) * theta);
-		MT_Scalar s1 = sin(t * theta);
-		
-		return d*(*this * s0 + q * s1);
+		MT_Scalar theta = acos(s);
+		d = MT_Scalar(1.0) / sin(theta);
+		s0 = sin((MT_Scalar(1.0) - t) * theta);
+		s1 = sin(t * theta);
 	}
 	else
 	{
-		return *this;
+		d = MT_Scalar(1.0);
+		s0 = MT_Scalar(1.0) - t;
+		s1 = t;
 	}
+	if (neg)
+		s1 = -s1;
+	return d*(*this * s0 + q * s1);
 }
 
diff --git a/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp b/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp
index 87ff3b53911..c3b0c21c8e0 100644
--- a/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp
+++ b/source/gameengine/Ketsji/KX_SG_NodeRelationships.cpp
@@ -235,23 +235,12 @@ UpdateChildCoordinates(
 			// now 'interpolate' the normal coordinates with the last 
 			// world coordinates to get the new world coordinates.
 
-			// problem 1:
-			// The child world scale needs to be initialized in some way for this 
-			// to make sense
-			// problem 2:
-			// This is way of doing interpolation is nonsense
-
-			int i;
-
 			MT_Scalar weight = MT_Scalar(1)/(m_relax + 1);
-			for (i=0;i <3 ;i++) {
-				child_w_scale[i] = (m_relax * child_w_scale[i] + child_n_scale[i]) * weight;
-				child_w_pos[i] = (m_relax * child_w_pos[i] + child_n_pos[i]) * weight;
-				child_w_rotation[0][i] = (m_relax * child_w_rotation[0][i] + child_n_rotation[0][i]) * weight;
-				child_w_rotation[1][i] = (m_relax * child_w_rotation[1][i] + child_n_rotation[1][i]) * weight;
-				child_w_rotation[2][i] = (m_relax * child_w_rotation[2][i] + child_n_rotation[2][i]) * weight;
-			}
-			
+			child_w_scale = (m_relax * child_w_scale + child_n_scale) * weight;
+			child_w_pos = (m_relax * child_w_pos + child_n_pos) * weight;
+			// for rotation we must go through quaternion
+			MT_Quaternion child_w_quat = child_w_rotation.getRotation().slerp(child_n_rotation.getRotation(), weight);
+			child_w_rotation.setRotation(child_w_quat);
 			//FIXME: update physics controller.
 		} else {
 			child_w_scale = child_n_scale;