translation widget

Former-commit-id: efff0ee92816de36b41916643f965b95fae2911e

include/igl/forward_kinematics.cpp

@@ -7,6 +7,7 @@
 // obtain one at http://mozilla.org/MPL/2.0/.
 #include "forward_kinematics.h"
 #include <functional>
+#include <iostream>
 
 IGL_INLINE void igl::forward_kinematics(
   const Eigen::MatrixXd & C,
@@ -14,6 +15,7 @@ IGL_INLINE void igl::forward_kinematics(
   const Eigen::VectorXi & P,
   const std::vector<
     Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+  const std::vector<Eigen::Vector3d> & dT,
   std::vector<
     Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
   std::vector<Eigen::Vector3d> & vT)
@@ -23,9 +25,11 @@ IGL_INLINE void igl::forward_kinematics(
   const int m = BE.rows(); 
   assert(m == P.rows());
   assert(m == (int)dQ.size());
+  assert(m == (int)dT.size());
   vector<bool> computed(m,false);
   vQ.resize(m);
   vT.resize(m);
+  // Dynamic programming
   function<void (int) > fk_helper = [&] (int b)
   {
     if(!computed[b])
@@ -35,7 +39,7 @@ IGL_INLINE void igl::forward_kinematics(
         // base case for roots
         vQ[b] = dQ[b];
         const Vector3d r = C.row(BE(b,0)).transpose();
-        vT[b] = r-dQ[b]*r;
+        vT[b] = r-dQ[b]*r + dT[b];
       }else
       {
         // Otherwise first compute parent's
@@ -43,7 +47,7 @@ IGL_INLINE void igl::forward_kinematics(
         fk_helper(p);
         vQ[b] = vQ[p] * dQ[b];
         const Vector3d r = C.row(BE(b,0)).transpose();
-        vT[b] = vT[p] - vQ[b]*r + vQ[p]*r;
+        vT[b] = vT[p] - vQ[b]*r + vQ[p]*(r + dT[b]);
       }
       computed[b] = true;
     }
@@ -60,13 +64,28 @@ IGL_INLINE void igl::forward_kinematics(
   const Eigen::VectorXi & P,
   const std::vector<
     Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+  std::vector<
+    Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
+  std::vector<Eigen::Vector3d> & vT)
+{
+  std::vector<Eigen::Vector3d> dT(BE.rows(),Eigen::Vector3d(0,0,0));
+  return forward_kinematics(C,BE,P,dQ,dT,vQ,vT);
+}
+
+IGL_INLINE void igl::forward_kinematics(
+  const Eigen::MatrixXd & C,
+  const Eigen::MatrixXi & BE,
+  const Eigen::VectorXi & P,
+  const std::vector<
+    Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+  const std::vector<Eigen::Vector3d> & dT,
   Eigen::MatrixXd & T)
 {
   using namespace Eigen;
   using namespace std;
   vector< Quaterniond,aligned_allocator<Quaterniond> > vQ;
   vector< Vector3d> vT;
-  forward_kinematics(C,BE,P,dQ,vQ,vT);
+  forward_kinematics(C,BE,P,dQ,dT,vQ,vT);
   const int dim = C.cols();
   T.resize(BE.rows()*(dim+1),dim);
   for(int e = 0;e<BE.rows();e++)
@@ -79,6 +98,18 @@ IGL_INLINE void igl::forward_kinematics(
   }
 }
 
+IGL_INLINE void igl::forward_kinematics(
+  const Eigen::MatrixXd & C,
+  const Eigen::MatrixXi & BE,
+  const Eigen::VectorXi & P,
+  const std::vector<
+    Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+  Eigen::MatrixXd & T)
+{
+  std::vector<Eigen::Vector3d> dT(BE.rows(),Eigen::Vector3d(0,0,0));
+  return forward_kinematics(C,BE,P,dQ,dT,T);
+}
+
 #ifdef IGL_STATIC_LIBRARY
 // Explicit template specialization
 #endif

include/igl/forward_kinematics.h

@@ -23,6 +23,7 @@ namespace igl
   //   BE  #BE by 2 list of bone edge indices
   //   P  #BE list of parent indices into BE
   //   dQ  #BE list of relative rotations
+  //   dT  #BE list of relative translations
   // Outputs:
   //   vQ  #BE list of absolute rotations
   //   vT  #BE list of absolute translations
@@ -32,9 +33,21 @@ namespace igl
     const Eigen::VectorXi & P,
     const std::vector<
       Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+    const std::vector<Eigen::Vector3d> & dT,
     std::vector<
       Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
     std::vector<Eigen::Vector3d> & vT);
+  // Wrapper assuming each dT[i] == {0,0,0}
+  IGL_INLINE void forward_kinematics(
+    const Eigen::MatrixXd & C,
+    const Eigen::MatrixXi & BE,
+    const Eigen::VectorXi & P,
+    const std::vector<
+      Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+    std::vector<
+      Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & vQ,
+    std::vector<Eigen::Vector3d> & vT);
+
   // Outputs:
   //   T  #BE*(dim+1) by dim stack of transposed transformation matrices
   IGL_INLINE void forward_kinematics(
@@ -43,7 +56,16 @@ namespace igl
     const Eigen::VectorXi & P,
     const std::vector<
       Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+    const std::vector<Eigen::Vector3d> & dT,
     Eigen::MatrixXd & T);
+  IGL_INLINE void forward_kinematics(
+    const Eigen::MatrixXd & C,
+    const Eigen::MatrixXi & BE,
+    const Eigen::VectorXi & P,
+    const std::vector<
+      Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> > & dQ,
+    Eigen::MatrixXd & T);
+
 };
 
 #ifndef IGL_STATIC_LIBRARY

include/igl/opengl2/MouseController.h

@@ -10,6 +10,7 @@
 // Needs to be included before others
 #include <Eigen/StdVector>
 #include "RotateWidget.h"
+#include "TranslateWidget.h"
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <vector>
@@ -48,6 +49,12 @@ namespace igl
           const Eigen::Vector4f & selected_color,
           const Eigen::Vector4f & unselected_color,
           Eigen::MatrixXf & C);
+        enum WidgetMode
+        {
+          WIDGET_MODE_ROTATE = 0,
+          WIDGET_MODE_TRANSLATE = 1,
+          NUM_WIDGET_MODES = 2,
+        };
       private:
         // m_is_selecting  whether currently selecting 
         // m_selection  #m_rotations list of whether a bone is selected
@@ -61,6 +68,7 @@ namespace igl
         // m_rotations  list of rotations for each bone
         // m_rotations_at_selection  list of rotations for each bone at time of
         //   selection
+        // m_translations   list of translations for each bone
         // m_fk_rotations_at_selection  list of rotations for each bone at time of
         //   selection
         // m_root_enabled  Whether root is enabled
@@ -69,13 +77,24 @@ namespace igl
         int m_down_x,m_down_y,m_drag_x,m_drag_y;
         int m_width,m_height;
         igl::opengl2::RotateWidget m_widget;
+        igl::opengl2::TranslateWidget m_trans_widget;
         Eigen::Quaterniond m_widget_rot_at_selection;
+        //Eigen::Vector3d m_trans_widget_trans_at_selection;
         typedef std::vector<
           Eigen::Quaterniond,
           Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;
+        typedef std::vector< Eigen::Vector3d > TranslationList;
         RotationList 
-          m_rotations,m_rotations_at_selection,m_fk_rotations_at_selection;
+          m_rotations,
+          m_rotations_at_selection,
+          m_fk_rotations_at_selection,
+          m_parent_rotations_at_selection;
+        TranslationList 
+          m_translations, 
+          m_translations_at_selection,
+          m_fk_translations_at_selection;
         bool m_root_enabled;
+        WidgetMode m_widget_mode;
       public:
         MouseController();
         // Returns const reference to m_selection
@@ -83,8 +102,10 @@ namespace igl
         //                          〃 m_is_selecting
         inline const bool & is_selecting() const{return m_is_selecting;}
         inline bool is_widget_down() const{return m_widget.is_down();}
+        inline bool is_trans_widget_down() const{return m_trans_widget.is_down();}
         //                          〃 m_rotations
         inline const RotationList & rotations() const{return m_rotations;}
+        inline const TranslationList & translations() const{return m_translations;}
         // Returns non-const reference to m_root_enabled
         inline bool & root_enabled(){ return m_root_enabled;}
         inline void reshape(const int w, const int h);
@@ -124,13 +145,19 @@ namespace igl
         //  n  number of bones
         inline void set_size(const int n);
         // Resets m_rotation elements to identity
+        inline void reset();
+        inline void reset_selected();
         inline void reset_rotations();
         inline void reset_selected_rotations();
+        inline void reset_translations();
+        inline void reset_selected_translations();
         inline bool set_rotations(const RotationList & vQ);
+        inline bool set_translations(const TranslationList & vT);
         // Sets all entries in m_selection to false
         inline void clear_selection();
         // Returns true iff some element in m_selection is true
         inline bool any_selection() const;
+        inline void set_widget_mode(const WidgetMode & mode);
       public:
           EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     };
@@ -202,9 +229,13 @@ inline igl::opengl2::MouseController::MouseController():
   m_width(-1),m_height(-1),
   m_widget(),
   m_widget_rot_at_selection(),
+  //m_trans_widget_trans_at_selection(),
+  m_trans_widget(),
   m_rotations(),
+  m_translations(),
   m_rotations_at_selection(),
-  m_root_enabled(true)
+  m_root_enabled(true),
+  m_widget_mode(WIDGET_MODE_ROTATE)
 {
 }
 
@@ -219,7 +250,12 @@ inline bool igl::opengl2::MouseController::down(const int x, const int y)
   using namespace std;
   m_down_x = m_drag_x =x;
   m_down_y = m_drag_y =y;
-  const bool widget_down = any_selection() && m_widget.down(x,m_height-y);
+  const bool widget_down = any_selection() && 
+    (
+     (m_widget_mode == WIDGET_MODE_ROTATE && m_widget.down(x,m_height-y)) ||
+     (m_widget_mode == WIDGET_MODE_TRANSLATE && 
+        m_trans_widget.down(x,m_height-y))
+    );
   if(!widget_down)
   {
     m_is_selecting = true;
@@ -238,46 +274,74 @@ inline bool igl::opengl2::MouseController::drag(const int x, const int y)
     return m_is_selecting;
   }else
   {
-    if(!m_widget.drag(x,m_height-y))
-    {
-      return false;
-    }
-    assert(any_selection());
-    assert(m_selection.size() == (int)m_rotations.size());
-    for(int e = 0;e<m_selection.size();e++)
+    switch(m_widget_mode)
     {
-      if(m_selection(e))
+      default: // fall through
+      case WIDGET_MODE_ROTATE:
       {
-        // Let:
-        //     w.θr = w.θ ⋅ w.θ₀*  
-        // w.θr takes (absolute) frame of w.θ₀ to w.θ:
-        //     w.θ = w.θr ⋅ w.θ₀ 
-        // Define:
-        //     w.θ₀ = θfk ⋅ θx,
-        // the absolute rotation of the x axis to the deformed bone at
-        // selection. Likewise,
-        //     w.θ = θfk' ⋅ θx,
-        // the current absolute rotation of the x axis to the deformed bone.
-        // Define recursively:
-        //     θfk = θfk(p) ⋅ Θr,
-        // then because we're only changeing this relative rotation
-        //     θfk' = θfk(p) ⋅ Θr ⋅ θr* ⋅ θr'
-        //     θfk' = θfk ⋅ θr* ⋅ θr'
-        //     w.θ ⋅ θx* = θfk ⋅ θr* ⋅ θr'
-        //     θr ⋅ θfk* ⋅ w.θ ⋅ θx* = θr'
-        //     θr ⋅ θfk* ⋅ w.θr ⋅ w.θ₀ ⋅ θx* = θr'
-        //     θr ⋅ θfk* ⋅ w.θr ⋅ θfk ⋅θx ⋅ θx* = θr'
-        //     θr ⋅ θfk* ⋅ w.θr ⋅ θfk = θr'
-        // which I guess is the right multiply change after being changed to
-        // the bases of θfk, the rotation of the bone relative to its rest
-        // frame.
-        //
-        const Quaterniond & frame = m_fk_rotations_at_selection[e];
-        m_rotations[e] = 
-          m_rotations_at_selection[e] *
-          frame.conjugate() * 
-          (m_widget.rot*m_widget_rot_at_selection.conjugate()) *
-          frame;
+        if(!m_widget.drag(x,m_height-y))
+        {
+          return false;
+        }
+        assert(any_selection());
+        assert(m_selection.size() == (int)m_rotations.size());
+        assert(m_selection.size() == (int)m_translations.size());
+        for(int e = 0;e<m_selection.size();e++)
+        {
+          if(m_selection(e))
+          {
+            // Let:
+            //     w.θr = w.θ ⋅ w.θ₀*  
+            // w.θr takes (absolute) frame of w.θ₀ to w.θ:
+            //     w.θ = w.θr ⋅ w.θ₀ 
+            // Define:
+            //     w.θ₀ = θfk ⋅ θx,
+            // the absolute rotation of the x axis to the deformed bone at
+            // selection. Likewise,
+            //     w.θ = θfk' ⋅ θx,
+            // the current absolute rotation of the x axis to the deformed bone.
+            // Define recursively:
+            //     θfk = θfk(p) ⋅ Θr,
+            // then because we're only changeing this relative rotation
+            //     θfk' = θfk(p) ⋅ Θr ⋅ θr* ⋅ θr'
+            //     θfk' = θfk ⋅ θr* ⋅ θr'
+            //     w.θ ⋅ θx* = θfk ⋅ θr* ⋅ θr'
+            //     θr ⋅ θfk* ⋅ w.θ ⋅ θx* = θr'
+            //     θr ⋅ θfk* ⋅ w.θr ⋅ w.θ₀ ⋅ θx* = θr'
+            //     θr ⋅ θfk* ⋅ w.θr ⋅ θfk ⋅θx ⋅ θx* = θr'
+            //     θr ⋅ θfk* ⋅ w.θr ⋅ θfk = θr'
+            // which I guess is the right multiply change after being changed to
+            // the bases of θfk, the rotation of the bone relative to its rest
+            // frame.
+            //
+            const Quaterniond & frame = m_fk_rotations_at_selection[e];
+            m_rotations[e] = 
+              m_rotations_at_selection[e] *
+              frame.conjugate() * 
+              (m_widget.rot*m_widget_rot_at_selection.conjugate()) *
+              frame;
+          }
+        }
+      }
+      case WIDGET_MODE_TRANSLATE:
+      {
+        if(!m_trans_widget.drag(x,m_height-y))
+        {
+          return false;
+        }
+        assert(any_selection());
+        assert(m_selection.size() == (int)m_rotations.size());
+        assert(m_selection.size() == (int)m_translations.size());
+        for(int e = 0;e<m_selection.size();e++)
+        {
+          if(m_selection(e))
+          {
+            m_translations[e] = 
+              m_translations_at_selection[e] + 
+              m_parent_rotations_at_selection[e].conjugate()*
+                m_trans_widget.m_trans;
+          }
+        }
       }
     }
     return true;
@@ -288,6 +352,7 @@ inline bool igl::opengl2::MouseController::up(const int x, const int y)
 {
   m_is_selecting = false;
   m_widget.up(x,m_height-y);
+  m_trans_widget.up(x,m_height-y);
   return false;
 }
 
@@ -295,7 +360,16 @@ inline void igl::opengl2::MouseController::draw() const
 {
   if(any_selection())
   {
-    m_widget.draw();
+    switch(m_widget_mode)
+    {
+      default:
+      case WIDGET_MODE_ROTATE:
+        m_widget.draw();
+        break;
+      case WIDGET_MODE_TRANSLATE:
+        m_trans_widget.draw();
+        break;
+    }
   }
   if(m_is_selecting)
   {
@@ -342,6 +416,7 @@ inline void igl::opengl2::MouseController::set_selection_from_last_drag(
   using namespace Eigen;
   using namespace std;
   m_rotations_at_selection = m_rotations;
+  m_translations_at_selection = m_translations;
   assert(BE.rows() == P.rows());
   m_selection = VectorXb::Zero(BE.rows());
   // m_rotation[e]  is the relative rotation stored at bone e (as seen by the
@@ -350,7 +425,7 @@ inline void igl::opengl2::MouseController::set_selection_from_last_drag(
   //   vQ[e] = vQ[p(e)] * m_rotation[e]
   vector<Quaterniond,aligned_allocator<Quaterniond> > vQ;
   vector<Vector3d> vT;
-  forward_kinematics(C,BE,P,m_rotations,vQ,vT);
+  forward_kinematics(C,BE,P,m_rotations,m_translations,vQ,vT);
   // Loop over deformed bones
   for(int e = 0;e<BE.rows();e++)
   {
@@ -380,8 +455,19 @@ inline void igl::opengl2::MouseController::set_selection(
   using namespace std;
   vector<Quaterniond,aligned_allocator<Quaterniond> > & vQ = 
     m_fk_rotations_at_selection;
-  vector<Vector3d> vT;
-  forward_kinematics(C,BE,P,m_rotations,vQ,vT);
+  vector<Vector3d> & vT = m_fk_translations_at_selection;
+  forward_kinematics(C,BE,P,m_rotations,m_translations,vQ,vT);
+  m_parent_rotations_at_selection.resize(
+    m_rotations.size(),Quaterniond::Identity());
+  for(size_t r = 0;r<vQ.size();r++)
+  {
+    if(P(r)>=0)
+    {
+      m_parent_rotations_at_selection[r] = vQ[P(r)];
+    }
+  }
+
+
   if(&m_selection != &S)
   {
     m_selection = S;
@@ -439,7 +525,8 @@ inline void igl::opengl2::MouseController::set_selection(
   if(any_of(m_selection))
   {
     // Taking average 
-    m_widget.pos.setConstant(0);
+    Vector3d avg_pos(0,0,0);
+    //m_trans_widget_trans_at_selection.setConstant(0);
     m_widget_rot_at_selection.coeffs().setConstant(0);
     m_widget.rot.coeffs().array().setConstant(0);
     Quaterniond cur_rot(0,0,0,0);
@@ -456,29 +543,37 @@ inline void igl::opengl2::MouseController::set_selection(
           Affine3d a = Affine3d::Identity();
           a.translate(vT[e]);
           a.rotate(vQ[e]);
-          m_widget.pos += a*s;
+          avg_pos += a*s;
         }
         // Rotation of x axis to this bone
         Quaterniond rot_at_bind;
         rot_at_bind.setFromTwoVectors(Vector3d(1,0,0),b);
         const Quaterniond abs_rot = vQ[e] * rot_at_bind;
         m_widget_rot_at_selection.coeffs() += abs_rot.coeffs();
+        //m_trans_widget_trans_at_selection += vT[e];
         num_selection++;
       }
     }
     // Take average
-    m_widget.pos.array() /= (double)num_selection;
+    avg_pos.array() /= (double)num_selection;
+    //m_trans_widget_trans_at_selection.array() /= (double)num_selection;
     m_widget_rot_at_selection.coeffs().array() /= (double)num_selection;
     m_widget_rot_at_selection.normalize();
     m_widget.rot = m_widget_rot_at_selection;
+    m_widget.pos      = avg_pos;
+    m_trans_widget.m_pos = avg_pos;
+    //m_trans_widget.m_trans = m_trans_widget_trans_at_selection;
+    m_trans_widget.m_trans.setConstant(0,0,0);
   }
   m_widget.m_is_enabled = true;
+  m_trans_widget.m_is_enabled = true;
   for(int s = 0;s<m_selection.rows();s++)
   {
     // a root is selected then disable.
     if(!m_root_enabled && m_selection(s) && P(s) == -1)
     {
       m_widget.m_is_enabled = false;
+      m_trans_widget.m_is_enabled = false;
       break;
     }
   }
@@ -490,9 +585,23 @@ inline void igl::opengl2::MouseController::set_size(const int n)
   clear_selection();
   m_rotations.clear();
   m_rotations.resize(n,Quaterniond::Identity());
+  m_translations.clear();
+  m_translations.resize(n,Vector3d(0,0,0));
   m_selection = VectorXb::Zero(n);
 }
 
+inline void igl::opengl2::MouseController::reset()
+{
+  reset_rotations();
+  reset_translations();
+}
+
+inline void igl::opengl2::MouseController::reset_selected()
+{
+  reset_selected_rotations();
+  reset_selected_translations();
+}
+
 inline void igl::opengl2::MouseController::reset_rotations()
 {
   using namespace Eigen;
@@ -514,6 +623,27 @@ inline void igl::opengl2::MouseController::reset_selected_rotations()
   }
 }
 
+inline void igl::opengl2::MouseController::reset_translations()
+{
+  using namespace Eigen;
+  using namespace std;
+  fill(m_translations.begin(),m_translations.end(),Vector3d(0,0,0));
+  // cop out. just clear selection
+  clear_selection();
+}
+
+inline void igl::opengl2::MouseController::reset_selected_translations()
+{
+  using namespace Eigen;
+  for(int e = 0;e<m_selection.size();e++)
+  {
+    if(m_selection(e))
+    {
+      m_translations[e] = Vector3d(0,0,0);
+    }
+  }
+}
+
 inline bool igl::opengl2::MouseController::set_rotations(const RotationList & vQ)
 {
   if(vQ.size() != m_rotations.size())
@@ -525,6 +655,17 @@ inline bool igl::opengl2::MouseController::set_rotations(const RotationList & vQ
   return true;
 }
 
+inline bool igl::opengl2::MouseController::set_translations(const TranslationList & vT)
+{
+  if(vT.size() != m_translations.size())
+  {
+    return false;
+  }
+  assert(!any_selection());
+  m_translations = vT;
+  return true;
+}
+
 inline void igl::opengl2::MouseController::clear_selection()
 {
   m_selection.setConstant(false);
@@ -535,4 +676,18 @@ inline bool igl::opengl2::MouseController::any_selection() const
   return igl::any_of(m_selection);
 }
 
+inline void igl::opengl2::MouseController::set_widget_mode(const WidgetMode & mode)
+{
+  switch(m_widget_mode)
+  {
+    default:
+    case WIDGET_MODE_TRANSLATE:
+      m_widget.pos = m_trans_widget.m_pos+m_trans_widget.m_trans;
+      break;
+    case WIDGET_MODE_ROTATE:
+      break;
+  }
+  m_widget_mode = mode;
+}
+
 #endif

include/igl/opengl2/RotateWidget.h

@@ -28,7 +28,6 @@ namespace igl
         inline static Eigen::Vector3d view_direction(const Eigen::Vector3d & pos);
         Eigen::Vector3d pos;
         Eigen::Quaterniond rot,down_rot;
-        // This line causes trouble if RotateWidget.h is included before bbw.h
         Eigen::Vector2d down_xy,drag_xy,down_dir;
         Eigen::Vector3d udown,udrag;
         double outer_radius_on_screen;

include/igl/opengl2/TranslateWidget.h

@@ -0,0 +1,211 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#ifndef IGL_OPENGL2_TRANSLATE_WIDGET_H
+#define IGL_OPENGL2_TRANSLATE_WIDGET_H
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+#include <vector>
+#include "../material_colors.h"
+
+namespace igl
+{
+  namespace opengl2
+  {
+    class TranslateWidget
+    {
+public:
+      // m_pos  position of center
+      // m_trans  translation vector
+      // m_down_xy  mouse position on down
+      // m_drag_xy  mouse position on drag
+      // m_is_enabled  whether enabled
+      Eigen::Vector3d m_pos,m_trans,m_down_trans;
+      Eigen::Vector2d m_down_xy, m_drag_xy;
+      bool m_is_enabled;
+      double m_len;
+      enum DownType
+      {
+        DOWN_TYPE_X = 0,
+        DOWN_TYPE_Y = 1,
+        DOWN_TYPE_Z = 2,
+        DOWN_TYPE_CENTER = 3,
+        DOWN_TYPE_NONE = 4,
+        NUM_DOWN_TYPES = 5
+      } m_down_type, m_selected_type;
+      inline TranslateWidget(const Eigen::Vector3d & pos = Eigen::Vector3d(0,0,0));
+      inline bool down(const int x, const int y);
+      inline bool drag(const int x, const int y);
+      inline bool up(const int x, const int y);
+      inline bool is_down() const;
+      inline void draw() const;
+public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    };
+  }
+}
+
+// Implementation
+#include "project.h"
+#include "unproject.h"
+
+inline igl::opengl2::TranslateWidget::TranslateWidget(
+  const Eigen::Vector3d & pos):
+  m_pos(pos),
+  m_trans(0,0,0),
+  m_down_xy(-1,-1),
+  m_drag_xy(-1,-1),
+  m_is_enabled(true),
+  m_len(50),
+  m_down_type(DOWN_TYPE_NONE), 
+  m_selected_type(DOWN_TYPE_NONE)
+{
+}
+
+inline bool igl::opengl2::TranslateWidget::down(const int x, const int y)
+{
+  using namespace Eigen;
+  using namespace std;
+  if(!m_is_enabled)
+  {
+    return false;
+  }
+  m_down_trans = m_trans;
+  m_down_xy = Vector2d(x,y);
+  m_drag_xy = m_down_xy;
+  m_down_type = DOWN_TYPE_NONE;
+  m_selected_type = DOWN_TYPE_NONE;
+  Vector3d ppos = project((m_pos+m_trans).eval());
+  const double r = (ppos.head(2) - m_down_xy).norm();
+  const double center_thresh = 10;
+  if(r < center_thresh)
+  {
+    m_down_type = DOWN_TYPE_CENTER;
+    m_selected_type = m_down_type;
+    return true;
+  }else if(r < m_len)
+  {
+    // Might be hit on lines
+  }
+  return false;
+}
+
+inline bool igl::opengl2::TranslateWidget::drag(const int x, const int y)
+{
+  using namespace std;
+  using namespace Eigen;
+  if(!m_is_enabled)
+  {
+    return false;
+  }
+  m_drag_xy = Vector2d(x,y);
+  switch(m_down_type)
+  {
+    case DOWN_TYPE_NONE:
+      return false;
+    default:
+    {
+      Vector3d ppos = project((m_pos+m_trans).eval());
+      Vector3d drag3(m_drag_xy(0),m_drag_xy(1),ppos(2));
+      Vector3d down3(m_down_xy(0),m_down_xy(1),ppos(2));
+      m_trans = m_down_trans + unproject(drag3)-unproject(down3);
+      return true;
+    }
+  }
+}
+
+inline bool igl::opengl2::TranslateWidget::up(const int /*x*/, const int /*y*/)
+{
+  // even if disabled process up
+  m_down_type = DOWN_TYPE_NONE;
+  return false;
+}
+
+inline bool igl::opengl2::TranslateWidget::is_down() const
+{
+  return m_down_type != DOWN_TYPE_NONE;
+}
+
+inline void igl::opengl2::TranslateWidget::draw() const
+{
+  using namespace Eigen;
+  using namespace std;
+  glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING_BIT | GL_DEPTH_BUFFER_BIT | GL_LINE_BIT);
+  glDisable(GL_LIGHTING);
+  glDisable(GL_DEPTH_TEST);
+  glLineWidth(2.0);
+  auto draw_axes = [&]()
+  {
+    glBegin(GL_LINES);
+    glColor3f(1,0,0);
+    glVertex3f(0,0,0);
+    glVertex3f(1,0,0);
+    glColor3f(0,1,0);
+    glVertex3f(0,0,0);
+    glVertex3f(0,1,0);
+    glColor3f(0,0,1);
+    glVertex3f(0,0,0);
+    glVertex3f(0,0,1);
+    glEnd();
+  };
+  auto draw_cube = []
+  {
+    glBegin(GL_LINES);
+    glVertex3f(-1.0f, 1.0f, 1.0f);
+    glVertex3f(1.0f, 1.0f, 1.0f);
+    glVertex3f(1.0f, 1.0f, 1.0f);
+    glVertex3f(1.0f, -1.0f, 1.0f);
+    glVertex3f(1.0f, -1.0f, 1.0f);
+    glVertex3f(-1.0f, -1.0f, 1.0f);
+    glVertex3f(-1.0f, -1.0f, 1.0f);
+    glVertex3f(-1.0f, 1.0f, 1.0f);
+    glVertex3f(1.0f, 1.0f, 1.0f);
+    glVertex3f(1.0f, 1.0f, -1.0f);
+    glVertex3f(1.0f, 1.0f, -1.0f);
+    glVertex3f(1.0f, -1.0f, -1.0f);
+    glVertex3f(1.0f, -1.0f, -1.0f);	
+    glVertex3f(1.0f, -1.0f, 1.0f);
+    glVertex3f(1.0f, 1.0f, -1.0f);
+    glVertex3f(-1.0f, 1.0f, -1.0f);
+    glVertex3f(-1.0f, -1.0f, -1.0f);
+    glVertex3f(1.0f, -1.0f, -1.0f);
+    glVertex3f(-1.0f, -1.0f, -1.0f);
+    glVertex3f(-1.0f, 1.0f, -1.0f);
+    glVertex3f(-1.0f, 1.0f, -1.0f);
+    glVertex3f(-1.0f, 1.0f, 1.0f);
+    glVertex3f(-1.0f, -1.0f, 1.0f);
+    glVertex3f(-1.0f, -1.0f, -1.0f);
+    glEnd();
+  };
+  glPushMatrix();
+  glTranslated( m_pos(0)+m_trans(0), m_pos(1)+m_trans(1), m_pos(2)+m_trans(2));
+
+  {
+    Vector3d off,ppos,ppos_off,pos_off;
+    project((m_pos+m_trans).eval(),ppos);
+    ppos_off = ppos;
+    ppos_off(0) += m_len;
+    unproject(ppos_off,pos_off);
+    const double r = (m_pos+m_trans-pos_off).norm();
+    glScaled(r,r,r);
+  }
+
+  draw_axes();
+  glScaled(0.05,0.05,0.05);
+  if(m_selected_type == DOWN_TYPE_CENTER)
+  {
+    glColor3fv(MAYA_YELLOW.data());
+  }else
+  {
+    glColor3fv(MAYA_GREY.data());
+  }
+  draw_cube();
+  glPopMatrix();
+  glPopAttrib();
+}
+
+#endif

include/igl/snap_to_fixed_up.cpp

@@ -29,4 +29,5 @@ IGL_INLINE void igl::snap_to_fixed_up(
 #ifdef IGL_STATIC_LIBRARY
 // Explicit template instanciations
 template void igl::snap_to_fixed_up<float>(Eigen::Quaternion<float, 0> const&, Eigen::Quaternion<float, 0>&);
+template void igl::snap_to_fixed_up<double>(Eigen::Quaternion<double, 0> const&, Eigen::Quaternion<double, 0>&);
 #endif