Torque Inverse Kinematics

Engine/source/math/mQuat.cpp

@@ -342,3 +342,40 @@ QuatF & QuatF::shortestArc( const VectorF &a, const VectorF &b )
    return *this;
 }
 
+QuatF& QuatF::computeRotationFromTo(const VectorF& from, const VectorF& to)
+{
+   VectorF f = from;
+   VectorF t = to;
+
+   f.normalizeSafe();
+   t.normalizeSafe();
+
+   if (f.isZero() || t.isZero())
+   {
+      return identity();
+   }
+
+   F32 dot = mClampF(mDot(f, t), -1.0f, 1.0f);
+
+   // Parallel = no rotation.
+   if (dot > 0.9999f)
+   {
+      return identity();
+   }
+
+   // Opposite = pick perpendicular.
+   if (dot < -0.9999f)
+   {
+      VectorF axis;
+      if (mFabs(f.x) < mFabs(f.z))
+         axis.set(0, -f.z, f.y);
+      else
+         axis.set(-f.y, f.x, 0);
+
+      axis.normalizeSafe();
+      return set(axis, M_PI_F); // 180 degrees
+   }
+
+   return shortestArc(f, t);
+}
+

Engine/source/math/mQuat.h

@@ -70,10 +70,12 @@ class QuatF
    QuatF& operator /=( F32 a );
 
    QuatF operator-( const QuatF &c ) const;
+   QuatF operator*(const QuatF& rhs) const;
    QuatF operator*( F32 a ) const;
 
    QuatF& square();
    QuatF& neg();
+   QuatF& conjugate();
    F32  dot( const QuatF &q ) const;
 
    MatrixF* setMatrix( MatrixF * mat ) const;
@@ -91,6 +93,7 @@ class QuatF
 
    // Vectors passed in must be normalized
    QuatF& shortestArc( const VectorF &normalizedA, const VectorF &normalizedB );
+   QuatF& computeRotationFromTo(const VectorF& from, const VectorF& to);
 };
 
 // a couple simple utility methods
@@ -208,6 +211,18 @@ inline QuatF QuatF::operator -( const QuatF &c ) const
                  w - c.w );
 }
 
+inline QuatF QuatF::operator*(const QuatF& rhs) const
+{
+   QuatF out;
+
+   out.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
+   out.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y;
+   out.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x;
+   out.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w;
+
+   return out;
+}
+
 inline QuatF QuatF::operator *( F32 a ) const
 {
    return QuatF( x * a,
@@ -225,6 +240,14 @@ inline QuatF& QuatF::neg()
    return *this;
 }
 
+inline QuatF& QuatF::conjugate()
+{
+   x = -x;
+   y = -y;
+   z = -z;
+   return *this;
+}
+
 inline F32 QuatF::dot( const QuatF &q ) const
 {
    return mClampF(w*q.w + x*q.x + y*q.y + z*q.z, -1.0f, 1.0f);

Engine/source/ts/tsAnimate.cpp

@@ -866,8 +866,21 @@ void TSShapeInstance::animate(S32 dl)
 
    // animate nodes?
    if (dirtyFlags & TransformDirty)
+   {
       animateNodes(ss);
 
+      //---------------------------------------
+      // TODO: Implement different ik methods
+      //       add limits to ik chain nodes
+      //       cache bone lengths.
+      //---------------------------------------
+      for (U32 i = 0; i < mShape->ikChains.size(); i++)
+      {
+         if (mShape->ikChains[i].enabled)
+            solveCCD(mShape->ikChains[i]);
+      }
+   }
+
    // animate objects?
    if (dirtyFlags & VisDirty)
       animateVisibility(ss);