dqs and lbs_matrix from puppet

Former-commit-id: db7b0550dbf0d4bfb80f7067e0b545eec6977d22

include/igl/dqs.cpp

@@ -0,0 +1,66 @@
+#include "dqs.h"
+#include <Eigen/Geometry>
+template <
+  typename DerivedV,
+  typename DerivedW,
+  typename Q,
+  typename T,
+  typename DerivedU>
+IGL_INLINE void igl::dqs(
+  const Eigen::PlainObjectBase<DerivedV> & V,
+  const Eigen::PlainObjectBase<DerivedW> & W,
+  const std::vector<Q> & vQ,
+  const std::vector<T> & vT,
+  Eigen::PlainObjectBase<DerivedU> & U)
+{
+  using namespace std;
+  using namespace igl;
+  assert(V.rows() <= W.rows());
+  assert(W.cols() == (int)vQ.size());
+  assert(W.cols() == (int)vT.size());
+  // resize output
+  U.resize(V.rows(),V.cols());
+
+  // Convert quats + trans into dual parts
+  vector<Q> vD(vQ.size());
+  for(int c = 0;c<W.cols();c++)
+  {
+    const Q & q = vQ[c];
+    vD[c].w() = -0.5*( vT[c](0)*q.x() + vT[c](1)*q.y() + vT[c](2)*q.z());
+    vD[c].x() =  0.5*( vT[c](0)*q.w() + vT[c](1)*q.z() - vT[c](2)*q.y());
+    vD[c].y() =  0.5*(-vT[c](0)*q.z() + vT[c](1)*q.w() + vT[c](2)*q.x());
+    vD[c].z() =  0.5*( vT[c](0)*q.y() - vT[c](1)*q.x() + vT[c](2)*q.w());
+  }
+
+  // Loop over vertices
+//#pragma omp parallel for
+  for(int i = 0;i<V.rows();i++)
+  {
+    Q b0(0,0,0,0);
+    Q be(0,0,0,0);
+    // Loop over handles
+    for(int c = 0;c<W.cols();c++)
+    {
+      b0.coeffs() += W(i,c) * vQ[c].coeffs();
+      be.coeffs() += W(i,c) * vD[c].coeffs();
+    }
+    Q ce = be;
+    ce.coeffs() /= b0.norm();
+    Q c0 = b0;
+    c0.coeffs() /= b0.norm();
+    // See algorithm 1 in "Geometric skinning with approximate dual quaternion
+    // blending" by Kavan et al
+    T v = V.row(i);
+    T d0 = c0.vec();
+    T de = ce.vec();
+    typename Q::Scalar a0 = c0.w();
+    typename Q::Scalar ae = ce.w();
+    U.row(i) =  v + 2*d0.cross(d0.cross(v) + a0*v) + 2*(a0*de - ae*d0 + d0.cross(de));
+  }
+
+}
+
+#ifndef IGL_HEADER_ONLY
+// Explicit template instanciation
+template void igl::dqs<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Quaternion<double, 0>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, std::vector<Eigen::Quaternion<double, 0>, std::allocator<Eigen::Quaternion<double, 0> > > const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+#endif

include/igl/dqs.h

@@ -0,0 +1,34 @@
+#ifndef IGL_DQS_H
+#define IGL_DQS_H
+#include "igl_inline.h"
+#include <vector>
+#include <Eigen/Core>
+namespace igl
+{
+  // Dual quaternion skinning
+  //
+  // Inputs:
+  //   V  #V by 3 list of rest positions
+  //   W  #W by #C list of weights
+  //   vQ  #C list of rotation quaternions 
+  //   vT  #C list of translation vectors
+  // Outputs:
+  //   U  #V by 3 list of new positions
+  template <
+    typename DerivedV,
+    typename DerivedW,
+    typename Q,
+    typename T,
+    typename DerivedU>
+  IGL_INLINE void dqs(
+    const Eigen::PlainObjectBase<DerivedV> & V,
+    const Eigen::PlainObjectBase<DerivedW> & W,
+    const std::vector<Q> & vQ,
+    const std::vector<T> & vT,
+    Eigen::PlainObjectBase<DerivedU> & U);
+};
+
+#ifdef IGL_HEADER_ONLY
+#  include "dqs.cpp"
+#endif
+#endif