#include "deform_skeleton.h"
void igl::deform_skeleton(
  const Eigen::MatrixXd & C,
  const Eigen::MatrixXi & BE,
  const std::vector<
    Eigen::Affine3d,Eigen::aligned_allocator<Eigen::Affine3d> > & vA,
  Eigen::MatrixXd & CT,
  Eigen::MatrixXi & BET)
{
  using namespace Eigen;
  assert(BE.rows() == (int)vA.size());
  CT.resize(2*BE.rows(),C.cols());
  BET.resize(BE.rows(),2);
  for(int e = 0;e<BE.rows();e++)
  {
    BET(e,0) = 2*e;
    BET(e,1) = 2*e+1;
    Affine3d a = vA[e];
    Vector3d c0 = C.row(BE(e,0));
    Vector3d c1 = C.row(BE(e,1));
    CT.row(2*e) =   a * c0;
    CT.row(2*e+1) = a * c1;
  }

}

IGL_INLINE void igl::deform_skeleton(
  const Eigen::MatrixXd & C,
  const Eigen::MatrixXi & BE,
  const Eigen::MatrixXd & T,
  Eigen::MatrixXd & CT,
  Eigen::MatrixXi & BET)
{
  using namespace Eigen;
  //assert(BE.rows() == (int)vA.size());
  CT.resize(2*BE.rows(),C.cols());
  BET.resize(BE.rows(),2);
  for(int e = 0;e<BE.rows();e++)
  {
    BET(e,0) = 2*e;
    BET(e,1) = 2*e+1;
    Affine3d a;
    a.matrix() = T.block(e*4,0,4,3).transpose();
    Vector3d c0 = C.row(BE(e,0));
    Vector3d c1 = C.row(BE(e,1));
    CT.row(2*e) =   a * c0;
    CT.row(2*e+1) = a * c1;
  }
}