forward_kinematics.h 2.5 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273
  1. // This file is part of libigl, a simple c++ geometry processing library.
  2. //
  3. // Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
  4. //
  5. // This Source Code Form is subject to the terms of the Mozilla Public License
  6. // v. 2.0. If a copy of the MPL was not distributed with this file, You can
  7. // obtain one at http://mozilla.org/MPL/2.0/.
  8. #ifndef IGL_FORWARD_KINEMATICS_H
  9. #define IGL_FORWARD_KINEMATICS_H
  10. #include "igl_inline.h"
  11. #include <Eigen/Core>
  12. #include <Eigen/Geometry>
  13. #include <vector>
  14. namespace igl
  15. {
  16. // Given a skeleton and a set of relative bone rotations compute absolute
  17. // rigid transformations for each bone.
  18. //
  19. // Inputs:
  20. // C #C by dim list of joint positions
  21. // BE #BE by 2 list of bone edge indices
  22. // P #BE list of parent indices into BE
  23. // dQ #BE list of relative rotations
  24. // dT #BE list of relative translations
  25. // Outputs:
  26. // vQ #BE list of absolute rotations
  27. // vT #BE list of absolute translations
  28. IGL_INLINE void forward_kinematics(
  29. const Eigen::MatrixXd & C,
  30. const Eigen::MatrixXi & BE,
  31. const Eigen::VectorXi & P,
  32. const std::vector<
  33. Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
  34. const std::vector<Eigen::Vector3d> & dT,
  35. std::vector<
  36. Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
  37. std::vector<Eigen::Vector3d> & vT);
  38. // Wrapper assuming each dT[i] == {0,0,0}
  39. IGL_INLINE void forward_kinematics(
  40. const Eigen::MatrixXd & C,
  41. const Eigen::MatrixXi & BE,
  42. const Eigen::VectorXi & P,
  43. const std::vector<
  44. Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
  45. std::vector<
  46. Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
  47. std::vector<Eigen::Vector3d> & vT);
  48. // Outputs:
  49. // T #BE*(dim+1) by dim stack of transposed transformation matrices
  50. IGL_INLINE void forward_kinematics(
  51. const Eigen::MatrixXd & C,
  52. const Eigen::MatrixXi & BE,
  53. const Eigen::VectorXi & P,
  54. const std::vector<
  55. Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
  56. const std::vector<Eigen::Vector3d> & dT,
  57. Eigen::MatrixXd & T);
  58. IGL_INLINE void forward_kinematics(
  59. const Eigen::MatrixXd & C,
  60. const Eigen::MatrixXi & BE,
  61. const Eigen::VectorXi & P,
  62. const std::vector<
  63. Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
  64. Eigen::MatrixXd & T);
  65. };
  66. #ifndef IGL_STATIC_LIBRARY
  67. # include "forward_kinematics.cpp"
  68. #endif
  69. #endif