void setTrajectoryKeyPosition(double * trajectoryKeyPositions, int * size1, int * size2)
{
  if (TRAJECTORYKEYPOSITIONS.size() != 0)
  {
    std::cout << "You are reinitializing TrajectoryKeyPositions...\n" ;
    for (unsigned int i = 0; i < TRAJECTORYKEYPOSITIONS.size(); i++)
      TRAJECTORYKEYPOSITIONS[i].resize(0);
    TRAJECTORYKEYPOSITIONS.resize(0);
  }

  TRAJECTORYKEYPOSITIONS.resize(*size1);
  for (int i = 0; i < *size1; i++)
  {
    TRAJECTORYKEYPOSITIONS[i].resize(*size2);
    for (int j = 0; j < *size2; j++)
      TRAJECTORYKEYPOSITIONS[i][j] = trajectoryKeyPositions[i * (*size2) + j];
  }
}
void getTrajectoryKeyPosition(double * trajectoryKeyPositions)
{
  for (unsigned int i = 0; i < TRAJECTORYKEYPOSITIONS.size(); i++)
    for (unsigned int j = 0; j < TRAJECTORYKEYPOSITIONS[0].size(); j++)
      trajectoryKeyPositions[i * TRAJECTORYKEYPOSITIONS[0].size() + j] = TRAJECTORYKEYPOSITIONS[i][j];
}
void getNDOF(int *size)
{
  *size = TRAJECTORYKEYPOSITIONS.size();
}