text: // Author: Dimitrios Tzionas
  //
  // source code for the work:
  //
  // Dimitrios Tzionas and Juergen Gall
  // 3D Object Reconstruction from Hand-Object Interactions
  // International Conference on Computer Vision (ICCV) 2015
  // http://files.is.tue.mpg.de/dtzionas/In-Hand-Scanning
  
  
  #include "animation.h"
  
  
  
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
  
  
  void Animation::poseMatrices_Clear()
  {
  
              ////////////////////////////////////////////////////////////////////////////////////////////////////
              ////////////////////////////////////////////////////////////////////////////////////////////////////
  
              for (int modelID=0; modelID<modelSet.totalModels; modelID++)
              {
  
                  ////////////////////////////////////////////////////////////////////
                  int NoOfFrames = movingModels[modelID].motionFrames.size();                if (NoOfFrames == 0)    return;
                  int NoOfBones  = movingModels[modelID].motionFrames[0].bones.size();
                  ////////////////////////////////////////////////////////////////////
  
                  for (int fr=0; fr<NoOfFrames; fr++)
                  {
                          for (int b=0; b<NoOfBones; b++)
                          {
                                  movingModels[modelID].motionFrames[fr].bones[b].name   = "";
                                  movingModels[modelID].motionFrames[fr].bones[b].R      = Eigen::MatrixXd::Zero(3,3);
                                  movingModels[modelID].motionFrames[fr].bones[b].T      = Eigen::MatrixXd::Zero(3,1);
                                  movingModels[modelID].motionFrames[fr].bones[b].RT_3x4 = Eigen::MatrixXd::Zero(3,4);
                                  movingModels[modelID].motionFrames[fr].bones[b].RT_4x4 = Eigen::MatrixXd::Zero(4,4);
                          }
                  }
              }
  
              ////////////////////////////////////////////////////////////////////////////////////////////////////
              ////////////////////////////////////////////////////////////////////////////////////////////////////
              transform_ModelSet_SKELETONS( 1 );   // den exei simasia ti tha valw, ola einai midenika!
              ////////////////////////////////////////////////////////////////////////////////////////////////////
              ////////////////////////////////////////////////////////////////////////////////////////////////////
  
  }
  
  
  
  
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
  
  
  void Animation::poseMatrices_LoadFromFile( QString shouldPrint )
  {
  
              for (int modelID=0; modelID<modelSet.totalModels; modelID++)
              {
  
  
                          ////////////////////////////////////////////////////////////////////////////////////
                          ////////////////////////////////////////////////////////////////////////////////////
                          int NoOfFrames = modelSet.models[modelID].totalSkeletonFrames;
                          int NoOfBones  = modelSet.models[modelID].totalBones;
                          qDebug() << "Animation::readFromTXT - NoOfFrames -" << NoOfFrames;
                          qDebug() << "Animation::readFromTXT - NoOfBones  -" << NoOfBones;
                          ////////////////////////////////////////////////////////////////////////////////////
                          ////////////////////////////////////////////////////////////////////////////////////
  
                          movingModels[modelID].motionFrames.clear();
                          movingModels[modelID].motionFrames.resize( NoOfFrames );
  
                          for     (int fr=0; fr<NoOfFrames; fr++)
                          {
                              movingModels[modelID].motionFrames[fr].bones.clear();
                              movingModels[modelID].motionFrames[fr].bones.resize( NoOfBones );
  
                              for (int b=0; b<NoOfBones; b++)
                              {
                                  movingModels[modelID].motionFrames[fr].bones[b].R      = Eigen::MatrixXd::Zero(3,3);
                                  movingModels[modelID].motionFrames[fr].bones[b].T      = Eigen::MatrixXd::Zero(3,1);
                                  movingModels[modelID].motionFrames[fr].bones[b].RT_3x4 = Eigen::MatrixXd::Zero(3,4);
                                  movingModels[modelID].motionFrames[fr].bones[b].RT_4x4 = Eigen::MatrixXd::Zero(4,4);
                              }
                          }
  
                          movingModels[modelID].motionFrames.squeeze();
  
                          ////////////////////////////////////////////////////////////////////////////////////
                          ////////////////////////////////////////////////////////////////////////////////////
  
                          QFile myFileREAD( movingModels[modelID].PATH_complete_Motion_INput );
                          myFileREAD.open(QIODevice::ReadOnly);
  
                          if(!myFileREAD.isOpen()){
                              qDebug() << "Animation::readFromTxt - Error, unable to open **" << movingModels[modelID].PATH_complete_Motion_INput << "** for input";
                              return;
                          }
  
                          QTextStream myStreamREAD(&myFileREAD);
  
                          ////////////////////////////////////////////////////////////////////////////////////
                          ////////////////////////////////////////////////////////////////////////////////////
  
                          for (int b=0; b<NoOfBones; b++)
                          {
  
                                  myStreamREAD >> movingModels[modelID].motionFrames[0].bones[b].name;
                                  if (            movingModels[modelID].motionFrames[0].bones[b].name == "Lower")
                                  {
                                      QString help1;
                                      QString help2;
                                      myStreamREAD >> help1;
                                      myStreamREAD >> help2;
                                      movingModels[modelID].motionFrames[0].bones[b].name = movingModels[modelID].motionFrames[0].bones[b].name + " " + help1 + " " + help2;
                                  }
  
                                  ////////////////////////////////////////////////////////////////////////////
                                  ////////////////////////////////////////////////////////////////////////////
                                                                                                                                                              \
                                  double inDouble;
  
                                  for (int fr=0; fr<NoOfFrames; fr++)
                                  {
  
                                          movingModels[modelID].motionFrames[fr].bones[b].name = movingModels[modelID].motionFrames[0].bones[b].name;
  
                                          for (int i=0; i<3; i++)
                                          {
                                              for (int j=0; j<3; j++)
                                              {
                                                  myStreamREAD >> inDouble;       movingModels[modelID].motionFrames[fr].bones[b].R(i,j) = inDouble;
                                              }
                                          }
                                          for (int i=0; i<3; i++)
                                          {
                                                  myStreamREAD >> inDouble;       movingModels[modelID].motionFrames[fr].bones[b].T(i) = inDouble;
                                          }
  
                                          /////////////////////////////////////////////////////////////////
                                          construct__RT_3x4__RT_4x4__basedOn__R_T__1bone( modelID, fr, b );
                                          /////////////////////////////////////////////////////////////////
  
                                  }
  
                          }
  
                          ////////////////////////////////////////////////////////////////////////////////////
                          ////////////////////////////////////////////////////////////////////////////////////
  
                          myFileREAD.close();
  
              } // for (int modelID=0; modelID<modelSet.totalModels; modelID++)
  
              ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
              ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
              ////////    PRINTER    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
              ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
              ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
              if (shouldPrint == "print")
              {
                      for (int modelID=0; modelID<modelSet.totalModels; modelID++)
                      {
                                  //////////////////////////////////////////////////////////////
                                  int NoOfFrames = modelSet.models[modelID].totalSkeletonFrames;
                                  int NoOfBones  = modelSet.models[modelID].totalBones;
                                  //////////////////////////////////////////////////////////////
  
                                  std::cout << "\n\n" << QString("RESULTS :)\n").toStdString() << std::endl;
  
                                  for (int b=0; b<NoOfBones; b++)
                                  {
  
                                          std::cout << "\n" << movingModels[modelID].motionFrames[0].bones[b].name.toStdString() << "\n" << std::endl;
                                          qDebug() << "Animation::readFromTXT\t" << movingModels[modelID].motionFrames[0].bones[b].name;
  
                                          ////////////////////////////////////////////////////////////////////////////
                                          ////////////////////////////////////////////////////////////////////////////
  
                                          for (int fr=0; fr<NoOfFrames; fr++)
                                          {
                                                  std::cout << movingModels[modelID].motionFrames[fr].bones[b].R << "\n" << std::endl;
                                                  std::cout << movingModels[modelID].motionFrames[fr].bones[b].T << "\n" << std::endl;
                                          }
  
                                  }
  
                      } // for (int modelID=0; modelID<modelSet.totalModels; modelID++)
  
              }
  
  }
  
  
  
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
  
  
  void Animation::construct__RT_3x4__RT_4x4__basedOn__R_T__1bone( int modelID, int fr, int b )
  {
  
      movingModels[modelID].motionFrames[fr].bones[b].RT_3x4 << movingModels[modelID].motionFrames[fr].bones[b].R, movingModels[modelID].motionFrames[fr].bones[b].T;
  
      movingModels[modelID].motionFrames[fr].bones[b].RT_4x4 << movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(0,0),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(0,1),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(0,2),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(0,3),
                                                                movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(1,0),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(1,1),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(1,2),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(1,3),
                                                                movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(2,0),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(2,1),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(2,2),   movingModels[modelID].motionFrames[fr].bones[b].RT_3x4(2,3),
                                                                0,                                                             0,                                                             0,                                                             1;
  
  }
  
  
  
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
  
  
  void Animation::poseMatrices_print()
  {
  
      for (int modelID=0; modelID<modelSet.totalModels; modelID++)
      {
                  //////////////////////////////////////////////////////////////
                  int NoOfFrames = modelSet.models[modelID].totalSkeletonFrames;
                  int NoOfBones  = modelSet.models[modelID].totalBones;
                  //////////////////////////////////////////////////////////////
  
                  std::cout << "\n\n" << QString("RESULTS :)\n").toStdString() << std::endl;
  
                  for (int b=0; b<NoOfBones; b++)
                  {
  
                          std::cout << "\n" << movingModels[modelID].motionFrames[0].bones[b].name.toStdString() << "\n" << std::endl;
                          qDebug() << "Animation::readFromTXT\t" << movingModels[modelID].motionFrames[0].bones[b].name;
  
                          ////////////////////////////////////////////////////////////////////////////
                          ////////////////////////////////////////////////////////////////////////////
  
                          for (int fr=0; fr<NoOfFrames; fr++)
                          {
                                  std::cout << movingModels[modelID].motionFrames[fr].bones[b].R << "\n" << std::endl;
                                  std::cout << movingModels[modelID].motionFrames[fr].bones[b].T << "\n" << std::endl;
                          }
  
                  }
  
      } // for (int modelID=0; modelID<modelSet.totalModels; modelID++)
  
  }
  
  
  void Animation::poseMatrices_print_minimal()
  {
      for (int modelID=0; modelID<modelSet.totalModels; modelID++)
      {
          qDebug() << "Animation::poseMatrices_print_minimal #######################################" << modelID << movingModels[modelID].PATH_complete_Motion_INput;
          qDebug() << "Animation::poseMatrices_print_minimal - totalSkeletonFrames -" << modelSet.models[modelID].totalSkeletonFrames;
          qDebug() << "Animation::poseMatrices_print_minimal - totalBones          -" << modelSet.models[modelID].totalBones;
      }
  }
  
  
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
  
  
  
  
  
  