proper bf support

Former-commit-id: a8305a28512044d19b7b85d560adf3328928b4eb

include/igl/readBF.cpp

@@ -3,15 +3,17 @@
 #include <vector>
 #include <cstdio>
 #include <fstream>
+#include <cassert>
+#include <functional>
 template < 
   typename DerivedWI,
   typename DerivedP,
-  typename DerivedC>
+  typename DerivedO>
 IGL_INLINE bool igl::readBF(
   const std::string & filename,
   Eigen::PlainObjectBase<DerivedWI> & WI,
   Eigen::PlainObjectBase<DerivedP> & P,
-  Eigen::PlainObjectBase<DerivedC> & C)
+  Eigen::PlainObjectBase<DerivedO> & O)
 {
   using namespace std;
   ifstream is(filename);
@@ -22,7 +24,7 @@ IGL_INLINE bool igl::readBF(
   string line;
   std::vector<typename DerivedWI::Scalar> vWI;
   std::vector<typename DerivedP::Scalar> vP;
-  std::vector<std::vector<typename DerivedC::Scalar> > vC;
+  std::vector<std::vector<typename DerivedO::Scalar> > vO;
   while(getline(is, line))
   {
     int wi,p;
@@ -33,14 +35,73 @@ IGL_INLINE bool igl::readBF(
     }
     vWI.push_back(wi);
     vP.push_back(p);
-    vC.push_back({cx,cy,cz});
+    vO.push_back({cx,cy,cz});
   }
   list_to_matrix(vWI,WI);
   list_to_matrix(vP,P);
-  list_to_matrix(vC,C);
+  list_to_matrix(vO,O);
+  return true;
+}
+
+template < 
+  typename DerivedWI,
+  typename DerivedbfP,
+  typename DerivedO,
+  typename DerivedC,
+  typename DerivedBE,
+  typename DerivedP>
+IGL_INLINE bool igl::readBF(
+  const std::string & filename,
+  Eigen::PlainObjectBase<DerivedWI> & WI,
+  Eigen::PlainObjectBase<DerivedbfP> & bfP,
+  Eigen::PlainObjectBase<DerivedO> & offsets,
+  Eigen::PlainObjectBase<DerivedC> & C,
+  Eigen::PlainObjectBase<DerivedBE> & BE,
+  Eigen::PlainObjectBase<DerivedP> & P)
+{
+  using namespace Eigen;
+  using namespace std;
+  if(!readBF(filename,WI,bfP,offsets))
+  {
+    return false;
+  }
+
+  C.resize(WI.rows(),3);
+  vector<bool> computed(C.rows(),false);
+  // better not be cycles in bfP
+  std::function<Eigen::RowVector3d(const int)> locate_tip;
+  locate_tip = 
+    [&offsets,&computed,&bfP,&locate_tip,&C](const int w)->Eigen::RowVector3d
+  {
+    if(w<0) return Eigen::RowVector3d(0,0,0);
+    if(computed[w]) return C.row(w);
+    computed[w] = true;
+    return C.row(w) = locate_tip(bfP(w)) + offsets.row(w);
+  };
+  int num_roots = (bfP.array() == -1).count();
+  BE.resize(WI.rows()-num_roots,2);
+  P.resize(BE.rows());
+  for(int c = 0;c<C.rows();c++)
+  {
+    locate_tip(c);
+    assert(c>=0);
+    // weight associated with this bone
+    const int wi = WI(c);
+    if(wi >= 0)
+    {
+      // index into C
+      const int p = bfP(c);
+      assert(p >= 0 && "No weights for roots allowed");
+      // index into BE
+      const int pwi = WI(p);
+      P(wi) = pwi;
+      BE(wi,0) = p;
+      BE(wi,1) = c;
+    }
+  }
   return true;
 }
 
 #ifdef IGL_STATIC_LIBRARY
-template bool igl::readBF<Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template bool igl::readBF<Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(std::__1::basic_string<char, std::__1::char_traits<char>, std::__1::allocator<char> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
 #endif

include/igl/readBF.h

@@ -11,17 +11,47 @@ namespace igl
   // Output:
   //   WI  #B list of unique weight indices
   //   P  #B list of parent indices into B, -1 for roots
-  //   C  #B list of tip positions
+  //   O  #B by 3 list of tip offset vectors from parent (or position for roots)
   // Returns true on success, false on errors
   template < 
     typename DerivedWI,
     typename DerivedP,
-    typename DerivedC>
+    typename DerivedO>
   IGL_INLINE bool readBF(
     const std::string & filename,
     Eigen::PlainObjectBase<DerivedWI> & WI,
     Eigen::PlainObjectBase<DerivedP> & P,
-    Eigen::PlainObjectBase<DerivedC> & C);
+    Eigen::PlainObjectBase<DerivedO> & O);
+  // Read bone forest into pure bone-skeleton format, expects only bones (no
+  // point handles), and that a root in the .bf <---> no weight attachment.
+  //
+  // Input:
+  //   file_name  path to .bf bones tree file
+  // Output:
+  //   WI  #B list of unique weight indices
+  //   P  #B list of parent indices into B, -1 for roots
+  //   O  #B by 3 list of tip offset vectors from parent (or position for roots)
+  //   C  #C by 3 list of absolute joint locations
+  //   BE  #BE by 3 list of bone indices into C, in order of weight index
+  //   P  #BE list of parent bone indices into BE, -1 means root bone
+  // Returns true on success, false on errors
+  //   
+  // See also: readTGF, bone_parents, forward_kinematics
+  template < 
+    typename DerivedWI,
+    typename DerivedbfP,
+    typename DerivedO,
+    typename DerivedC,
+    typename DerivedBE,
+    typename DerivedP>
+  IGL_INLINE bool readBF(
+    const std::string & filename,
+    Eigen::PlainObjectBase<DerivedWI> & WI,
+    Eigen::PlainObjectBase<DerivedbfP> & bfP,
+    Eigen::PlainObjectBase<DerivedO> & O,
+    Eigen::PlainObjectBase<DerivedC> & C,
+    Eigen::PlainObjectBase<DerivedBE> & BE,
+    Eigen::PlainObjectBase<DerivedP> & P);
 }
 
 #ifndef IGL_STATIC_LIBRARY

include/igl/writeBF.cpp

@@ -0,0 +1,42 @@
+#include "writeBF.h"
+#include <fstream>
+#include <cassert>
+template < 
+  typename DerivedWI,
+  typename DerivedP,
+  typename DerivedO>
+IGL_INLINE bool igl::writeBF(
+  const std::string & filename,
+  const Eigen::PlainObjectBase<DerivedWI> & WI,
+  const Eigen::PlainObjectBase<DerivedP> & P,
+  const Eigen::PlainObjectBase<DerivedO> & O)
+{
+  using namespace Eigen;
+  using namespace std;
+  const int n = WI.rows();
+  assert(n == WI.rows() && "WI must have n rows");
+  assert(n == P.rows()  && "P must have n rows");
+  assert(n == O.rows()  && "O must have n rows");
+  MatrixXd WIPO(n,1+1+3);
+  for(int i = 0;i<n;i++)
+  {
+    WIPO(i,0) = WI(i);
+    WIPO(i,1) = P(i);
+    WIPO(i,2+0) = O(i,0);
+    WIPO(i,2+1) = O(i,1);
+    WIPO(i,2+2) = O(i,2);
+  }
+  ofstream s(filename);
+  if(!s.is_open())
+  {
+    fprintf(stderr,"IOError: writeBF() could not open %s\n",filename.c_str());
+    return false;
+  }
+  s<<
+    WIPO.format(IOFormat(FullPrecision,DontAlignCols," ","\n","","","","\n"));
+  return true;
+}
+
+#ifdef IGL_STATIC_LIBRARY
+template bool igl::writeBF<Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::__1::basic_string<char, std::__1::char_traits<char>, std::__1::allocator<char> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&);
+#endif

include/igl/writeBF.h

@@ -0,0 +1,30 @@
+#ifndef IGL_WRITEBF_H
+#define IGL_WRITEBF_H
+#include "igl_inline.h"
+#include <Eigen/Core>
+#include <string>
+namespace igl
+{
+  // Write a bones forest to a file
+  //
+  // Input:
+  //   file_name  path to .bf bones tree file
+  //   WI  #B list of unique weight indices
+  //   P  #B list of parent indices into B, -1 for roots
+  //   O  #B list of tip offsets
+  // Returns true on success, false on errors
+  template < 
+    typename DerivedWI,
+    typename DerivedP,
+    typename DerivedO>
+  IGL_INLINE bool writeBF(
+    const std::string & filename,
+    const Eigen::PlainObjectBase<DerivedWI> & WI,
+    const Eigen::PlainObjectBase<DerivedP> & P,
+    const Eigen::PlainObjectBase<DerivedO> & O);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "writeBF.cpp"
+#endif
+#endif