draw_skeleton_3d.cpp 5.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174
  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_NO_OPENGL
  9. #include "draw_skeleton_3d.h"
  10. #include "PI.h"
  11. #include "OpenGL_convenience.h"
  12. #include "material_colors.h"
  13. #include <Eigen/Geometry>
  14. #include <iostream>
  15. template <
  16. typename DerivedC,
  17. typename DerivedBE,
  18. typename DerivedT,
  19. typename Derivedcolor>
  20. IGL_INLINE void igl::draw_skeleton_3d(
  21. const Eigen::PlainObjectBase<DerivedC> & C,
  22. const Eigen::PlainObjectBase<DerivedBE> & BE,
  23. const Eigen::PlainObjectBase<DerivedT> & T,
  24. const Eigen::PlainObjectBase<Derivedcolor> & color,
  25. const double half_bbd)
  26. {
  27. // Note: Maya's skeleton *does* scale with the mesh suggesting a scale
  28. // parameter. Further, its joint balls are not rotated with the bones.
  29. using namespace Eigen;
  30. using namespace std;
  31. if(color.size() == 0)
  32. {
  33. return draw_skeleton_3d(C,BE,T,MAYA_SEA_GREEN,half_bbd);
  34. }
  35. assert(color.cols() == 4 || color.size() == 4);
  36. if(T.size() == 0)
  37. {
  38. typedef Eigen::Matrix<typename DerivedT::Scalar,Dynamic,Dynamic> Mat;
  39. Mat I = Mat::Identity(C.cols()+1,C.cols());
  40. Mat T = I.replicate(BE.rows(),1);
  41. // insane base case
  42. if(T.size() == 0)
  43. {
  44. return;
  45. }
  46. return draw_skeleton_3d(C,BE,T,color,half_bbd);
  47. }
  48. assert(T.rows() == BE.rows()*(C.cols()+1));
  49. assert(T.cols() == C.cols());
  50. // push old settings
  51. glPushAttrib(GL_LIGHTING_BIT);
  52. glPushAttrib(GL_LINE_BIT);
  53. glDisable(GL_LIGHTING);
  54. glLineWidth(1.0);
  55. auto draw_sphere = [](const double r)
  56. {
  57. auto draw_circle = []()
  58. {
  59. glBegin(GL_LINE_STRIP);
  60. for(double th = 0;th<2.0*igl::PI;th+=(2.0*igl::PI/30.0))
  61. {
  62. glVertex3d(cos(th),sin(th),0.0);
  63. }
  64. glVertex3d(cos(0),sin(0),0.0);
  65. glEnd();
  66. };
  67. glPushMatrix();
  68. glScaled(r,r,r);
  69. draw_circle();
  70. glRotated(90.0,1.0,0.0,0.0);
  71. draw_circle();
  72. glRotated(90.0,0.0,1.0,0.0);
  73. draw_circle();
  74. glPopMatrix();
  75. };
  76. auto draw_pyramid = [](const double r)
  77. {
  78. glBegin(GL_LINE_STRIP);
  79. glVertex3d(0, 1,-1);
  80. glVertex3d(0,-1,-1);
  81. glVertex3d(0,-1, 1);
  82. glVertex3d(0, 1, 1);
  83. glVertex3d(0, 1,-1);
  84. glEnd();
  85. glBegin(GL_LINES);
  86. glVertex3d(0, 1,-1);
  87. glVertex3d(1,0,0);
  88. glVertex3d(0,-1,-1);
  89. glVertex3d(1,0,0);
  90. glVertex3d(0,-1, 1);
  91. glVertex3d(1,0,0);
  92. glVertex3d(0, 1, 1);
  93. glVertex3d(1,0,0);
  94. glEnd();
  95. };
  96. // Loop over bones
  97. for(int e = 0;e < BE.rows();e++)
  98. {
  99. // Draw a sphere
  100. auto s = C.row(BE(e,0));
  101. auto d = C.row(BE(e,1));
  102. auto b = (d-s).transpose().eval();
  103. double r = 0.02*half_bbd;
  104. Matrix4d Te = Matrix4d::Identity();
  105. Te.block(0,0,3,4) = T.block(e*4,0,4,3).transpose();
  106. Quaterniond q;
  107. q.setFromTwoVectors(Vector3d(1,0,0),b);
  108. glPushMatrix();
  109. glMultMatrixd(Te.data());
  110. glTranslated(s(0),s(1),s(2));
  111. if(color.size() == 4)
  112. {
  113. glColor4d( color(0), color(1), color(2), color(3));
  114. }else
  115. {
  116. glColor4d( color(e,0), color(e,1), color(e,2), color(e,3));
  117. }
  118. draw_sphere(r);
  119. const double len = b.norm()-2.*r;
  120. if(len>=0)
  121. {
  122. auto u = b.normalized()*r;
  123. glPushMatrix();
  124. glTranslated(u(0),u(1),u(2));
  125. glMultMatrixd(Affine3d(q).matrix().data());
  126. glScaled(b.norm()-2.*r,r,r);
  127. draw_pyramid(r);
  128. glPopMatrix();
  129. }
  130. glTranslated(b(0),b(1),b(2));
  131. draw_sphere(r);
  132. glPopMatrix();
  133. }
  134. // Reset settings
  135. glPopAttrib();
  136. glPopAttrib();
  137. }
  138. template <typename DerivedC, typename DerivedBE, typename DerivedT>
  139. IGL_INLINE void igl::draw_skeleton_3d(
  140. const Eigen::PlainObjectBase<DerivedC> & C,
  141. const Eigen::PlainObjectBase<DerivedBE> & BE,
  142. const Eigen::PlainObjectBase<DerivedT> & T)
  143. {
  144. return draw_skeleton_3d(C,BE,T,MAYA_SEA_GREEN);
  145. }
  146. template <typename DerivedC, typename DerivedBE>
  147. IGL_INLINE void igl::draw_skeleton_3d(
  148. const Eigen::PlainObjectBase<DerivedC> & C,
  149. const Eigen::PlainObjectBase<DerivedBE> & BE)
  150. {
  151. return draw_skeleton_3d(C,BE,Eigen::MatrixXd(),MAYA_SEA_GREEN);
  152. }
  153. #endif
  154. #ifdef IGL_STATIC_LIBRARY
  155. #ifndef IGL_NO_OPENGL
  156. // Explicit template instanciation
  157. template void igl::draw_skeleton_3d<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&);
  158. template void igl::draw_skeleton_3d<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&);
  159. template void igl::draw_skeleton_3d<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<float, 4, 1, 0, 4, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, double);
  160. template void igl::draw_skeleton_3d<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> > const&, double);
  161. #endif
  162. #endif