Catmull-Rom

gistfile1.cpp

double catmullrom(double t, double p0, double p1, double p2, double p3, int m1, int m2)
{
  std::vector<std::vector<double> > t_vec(1, std::vector<double>(4, 0.0));
  t_vec[0][0] = t * t * t;
  t_vec[0][1] = t * t;
  t_vec[0][2] = t;
  t_vec[0][3] = 1;
  Matrix t_matrix(1, 4, t_vec);

  std::cout << "+++ Time Matrix +++\n" << t_matrix << std::endl << std::endl;

  std::vector<std::vector<double> > hermite_vec(4, std::vector<double>(4, 0.0));
  hermite_vec[0][0] = 2; hermite_vec[0][1] = -2; hermite_vec[0][2] = 1; hermite_vec[0][3] = 1;
  hermite_vec[1][0] = -3; hermite_vec[1][1] = 3; hermite_vec[1][2] = -2; hermite_vec[1][3] = -1;
  hermite_vec[2][0] = 0; hermite_vec[2][1] = 0; hermite_vec[2][2] = 1; hermite_vec[2][3] = 0;
  hermite_vec[3][0] = 1; hermite_vec[3][1] = 0; hermite_vec[3][2] = 0; hermite_vec[3][3] = 0;
  Matrix hermite_matrix(4, 4, hermite_vec);

  std::cout << "+++ Hermite Matrix +++\n" << hermite_matrix << std::endl << std::endl;

  std::vector<std::vector<double> > catmull_vec(4, std::vector<double>(4, 0.0));
  catmull_vec[0][m1] = 1;
  catmull_vec[1][m2] = 1;
  catmull_vec[2][std::max(m1 - 1, 0)] = -1; catmull_vec[2][std::min(m1 + 1, 3)] = 1;
  catmull_vec[3][std::max(m2 - 1, 0)] = -1; catmull_vec[3][std::min(m2 + 1, 3)] = 1;
  Matrix catmull_matrix(4, 4, catmull_vec);

  std::cout << "+++ Catmull Matrix +++\n" << catmull_matrix << std::endl << std::endl;

  std::vector<std::vector<double> > point_vec(4, std::vector<double>(1, 0.0));
  point_vec[0][0] = p0;
  point_vec[1][0] = p1;
  point_vec[2][0] = p2;
  point_vec[3][0] = p3;
  Matrix point_matrix(4, 1, point_vec);

  std::cout << "+++ Point Matrix +++\n" << point_matrix << std::endl << std::endl;

  Matrix temp0 = t_matrix * hermite_matrix;
  Matrix temp1 = temp0 * catmull_matrix;
  Matrix result = temp1 * point_matrix;

  return result.GetValue(0, 0);
}