matrix to quaternion converter and draw beach ball glyph from anttweakbar

Former-commit-id: 48040cb9b902e287197ed3ed720f06ff50c1d99e

include/igl/ReAntTweakBar.cpp

@@ -214,7 +214,8 @@ bool igl::ReTwBar::save(const char *file_name)
     it != rw_items.end(); 
     it++)
   {
-    const char * name = (*it).name.c_str();
+    std::string s = (*it).name;
+    const char * name = s.c_str();
     TwType type = (*it).type;
     void * var = (*it).var;
     fprintf(fp,"%s: %s\n",

include/igl/draw_beach_ball.cpp

@@ -0,0 +1,264 @@
+#include "draw_beach_ball.h"
+
+#include <OpenGL/GL.h>
+
+#include <vector>
+#include <cmath>
+#include <iostream>
+
+// Most of this implementation comes from the AntTweakBar source code:
+// TwMgr.cpp, TwMgr.h, TwColor.h, TwColor.cpp, TwOpenGL.h and TwOpenGL.cpp
+
+////////////////////////////////////////////////////////////////////////////
+// Begin Copied Straight from AntTweakBar
+////////////////////////////////////////////////////////////////////////////
+static const float  FLOAT_EPS     = 1.0e-7f;
+static const float  FLOAT_EPS_SQ  = 1.0e-14f;
+static const float  FLOAT_PI      = 3.14159265358979323846f;
+enum EArrowParts     { ARROW_CONE, ARROW_CONE_CAP, ARROW_CYL, ARROW_CYL_CAP };
+
+template <typename _T> inline const _T& TClamp(const _T& _X, const _T& _Limit1, const _T& _Limit2)
+{
+    if( _Limit1<_Limit2 )
+        return (_X<=_Limit1) ? _Limit1 : ( (_X>=_Limit2) ? _Limit2 : _X );
+    else
+        return (_X<=_Limit2) ? _Limit2 : ( (_X>=_Limit1) ? _Limit1 : _X );
+}
+
+typedef unsigned int color32;
+static inline color32 Color32FromARGBi(int _A, int _R, int _G, int _B)
+{
+    return (((color32)TClamp(_A, 0, 255))<<24) | (((color32)TClamp(_R, 0, 255))<<16) | (((color32)TClamp(_G, 0, 255))<<8) | ((color32)TClamp(_B, 0, 255));
+}
+
+static inline color32 Color32FromARGBf(float _A, float _R, float _G, float _B)
+{
+    return (((color32)TClamp(_A*256.0f, 0.0f, 255.0f))<<24) | (((color32)TClamp(_R*256.0f, 0.0f, 255.0f))<<16) | (((color32)TClamp(_G*256.0f, 0.0f, 255.0f))<<8) | ((color32)TClamp(_B*256.0f, 0.0f, 255.0f));
+}
+
+static inline void Color32ToARGBi(color32 _Color, int *_A, int *_R, int *_G, int *_B)
+{
+    if(_A) *_A = (_Color>>24)&0xff;
+    if(_R) *_R = (_Color>>16)&0xff;
+    if(_G) *_G = (_Color>>8)&0xff;
+    if(_B) *_B = _Color&0xff;
+}
+
+static inline void Color32ToARGBf(color32 _Color, float *_A, float *_R, float *_G, float *_B)
+{
+    if(_A) *_A = (1.0f/255.0f)*float((_Color>>24)&0xff);
+    if(_R) *_R = (1.0f/255.0f)*float((_Color>>16)&0xff);
+    if(_G) *_G = (1.0f/255.0f)*float((_Color>>8)&0xff);
+    if(_B) *_B = (1.0f/255.0f)*float(_Color&0xff);
+}
+
+static color32 ColorBlend(color32 _Color1, color32 _Color2, float _S)
+{
+    float a1, r1, g1, b1, a2, r2, g2, b2;
+    Color32ToARGBf(_Color1, &a1, &r1, &g1, &b1);
+    Color32ToARGBf(_Color2, &a2, &r2, &g2, &b2);
+    float t = 1.0f-_S;
+    return Color32FromARGBf(t*a1+_S*a2, t*r1+_S*r2, t*g1+_S*g2, t*b1+_S*b2);
+}
+static std::vector<float>   s_SphTri;
+static std::vector<color32> s_SphCol;
+static void CreateSphere()
+{
+    const int SUBDIV = 7;
+    s_SphTri.clear();
+    s_SphCol.clear();
+
+    const float A[8*3] = { 1,0,0, 0,0,-1, -1,0,0, 0,0,1,   0,0,1,  1,0,0,  0,0,-1, -1,0,0 };
+    const float B[8*3] = { 0,1,0, 0,1,0,  0,1,0,  0,1,0,   0,-1,0, 0,-1,0, 0,-1,0, 0,-1,0 };
+    const float C[8*3] = { 0,0,1, 1,0,0,  0,0,-1, -1,0,0,  1,0,0,  0,0,-1, -1,0,0, 0,0,1  };
+    //const color32 COL_A[8] = { 0xffff8080, 0xff000080, 0xff800000, 0xff8080ff,  0xff8080ff, 0xffff8080, 0xff000080, 0xff800000 };
+    //const color32 COL_B[8] = { 0xff80ff80, 0xff80ff80, 0xff80ff80, 0xff80ff80,  0xff008000, 0xff008000, 0xff008000, 0xff008000 };
+    //const color32 COL_C[8] = { 0xff8080ff, 0xffff8080, 0xff000080, 0xff800000,  0xffff8080, 0xff000080, 0xff800000, 0xff8080ff };
+    const color32 COL_A[8] = { 0xffffffff, 0xffffff40, 0xff40ff40, 0xff40ffff,  0xffff40ff, 0xffff4040, 0xff404040, 0xff4040ff };
+    const color32 COL_B[8] = { 0xffffffff, 0xffffff40, 0xff40ff40, 0xff40ffff,  0xffff40ff, 0xffff4040, 0xff404040, 0xff4040ff };
+    const color32 COL_C[8] = { 0xffffffff, 0xffffff40, 0xff40ff40, 0xff40ffff,  0xffff40ff, 0xffff4040, 0xff404040, 0xff4040ff };
+
+    int i, j, k, l;
+    float xa, ya, za, xb, yb, zb, xc, yc, zc, x, y, z, norm, u[3], v[3];
+    color32 col;
+    for( i=0; i<8; ++i )
+    {
+        xa = A[3*i+0]; ya = A[3*i+1]; za = A[3*i+2];
+        xb = B[3*i+0]; yb = B[3*i+1]; zb = B[3*i+2];
+        xc = C[3*i+0]; yc = C[3*i+1]; zc = C[3*i+2];
+        for( j=0; j<=SUBDIV; ++j )
+            for( k=0; k<=2*(SUBDIV-j); ++k )
+            {
+                if( k%2==0 )
+                {
+                    u[0] = ((float)j)/(SUBDIV+1);
+                    v[0] = ((float)(k/2))/(SUBDIV+1);
+                    u[1] = ((float)(j+1))/(SUBDIV+1);
+                    v[1] = ((float)(k/2))/(SUBDIV+1);
+                    u[2] = ((float)j)/(SUBDIV+1);
+                    v[2] = ((float)(k/2+1))/(SUBDIV+1);
+                }
+                else
+                {
+                    u[0] = ((float)j)/(SUBDIV+1);
+                    v[0] = ((float)(k/2+1))/(SUBDIV+1);
+                    u[1] = ((float)(j+1))/(SUBDIV+1);
+                    v[1] = ((float)(k/2))/(SUBDIV+1);
+                    u[2] = ((float)(j+1))/(SUBDIV+1);
+                    v[2] = ((float)(k/2+1))/(SUBDIV+1);
+                }
+
+                for( l=0; l<3; ++l )
+                {
+                    x = (1.0f-u[l]-v[l])*xa + u[l]*xb + v[l]*xc;
+                    y = (1.0f-u[l]-v[l])*ya + u[l]*yb + v[l]*yc;
+                    z = (1.0f-u[l]-v[l])*za + u[l]*zb + v[l]*zc;
+                    norm = sqrtf(x*x+y*y+z*z);
+                    x /= norm; y /= norm; z /= norm;
+                    s_SphTri.push_back(x); s_SphTri.push_back(y); s_SphTri.push_back(z);
+                    if( u[l]+v[l]>FLOAT_EPS )
+                        col = ColorBlend(COL_A[i], ColorBlend(COL_B[i], COL_C[i], v[l]/(u[l]+v[l])), u[l]+v[l]);
+                    else
+                        col = COL_A[i];
+                    //if( (j==0 && k==0) || (j==0 && k==2*SUBDIV) || (j==SUBDIV && k==0) )
+                    //  col = 0xffff0000;
+                    s_SphCol.push_back(col);
+                }
+            }
+    }
+    //s_SphTriProj.clear();
+    //s_SphTriProj.resize(2*s_SphCol.size(), 0);
+    //s_SphColLight.clear();
+    //s_SphColLight.resize(s_SphCol.size(), 0);
+}
+
+static std::vector<float> s_ArrowTri[4];
+static std::vector<float> s_ArrowNorm[4];
+static void CreateArrow()
+{
+    const int   SUBDIV  = 15;
+    const float CYL_RADIUS  = 0.08f;
+    const float CONE_RADIUS = 0.16f;
+    const float CONE_LENGTH = 0.25f;
+    const float ARROW_BGN = -1.1f;
+    const float ARROW_END = 1.15f;
+    int i;
+    for(i=0; i<4; ++i)
+    {
+        s_ArrowTri[i].clear();
+        s_ArrowNorm[i].clear();
+    }
+    
+    float x0, x1, y0, y1, z0, z1, a0, a1, nx, nn;
+    for(i=0; i<SUBDIV; ++i)
+    {
+        a0 = 2.0f*FLOAT_PI*(float(i))/SUBDIV;
+        a1 = 2.0f*FLOAT_PI*(float(i+1))/SUBDIV;
+        x0 = ARROW_BGN;
+        x1 = ARROW_END-CONE_LENGTH;
+        y0 = cosf(a0);
+        z0 = sinf(a0);
+        y1 = cosf(a1);
+        z1 = sinf(a1);
+        s_ArrowTri[ARROW_CYL].push_back(x1); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*y0); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*z0);
+        s_ArrowTri[ARROW_CYL].push_back(x0); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*y0); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*z0);
+        s_ArrowTri[ARROW_CYL].push_back(x0); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*y1); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*z1);
+        s_ArrowTri[ARROW_CYL].push_back(x1); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*y0); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*z0);
+        s_ArrowTri[ARROW_CYL].push_back(x0); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*y1); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*z1);
+        s_ArrowTri[ARROW_CYL].push_back(x1); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*y1); s_ArrowTri[ARROW_CYL].push_back(CYL_RADIUS*z1);
+        s_ArrowNorm[ARROW_CYL].push_back(0); s_ArrowNorm[ARROW_CYL].push_back(y0); s_ArrowNorm[ARROW_CYL].push_back(z0);
+        s_ArrowNorm[ARROW_CYL].push_back(0); s_ArrowNorm[ARROW_CYL].push_back(y0); s_ArrowNorm[ARROW_CYL].push_back(z0);
+        s_ArrowNorm[ARROW_CYL].push_back(0); s_ArrowNorm[ARROW_CYL].push_back(y1); s_ArrowNorm[ARROW_CYL].push_back(z1);
+        s_ArrowNorm[ARROW_CYL].push_back(0); s_ArrowNorm[ARROW_CYL].push_back(y0); s_ArrowNorm[ARROW_CYL].push_back(z0);
+        s_ArrowNorm[ARROW_CYL].push_back(0); s_ArrowNorm[ARROW_CYL].push_back(y1); s_ArrowNorm[ARROW_CYL].push_back(z1);
+        s_ArrowNorm[ARROW_CYL].push_back(0); s_ArrowNorm[ARROW_CYL].push_back(y1); s_ArrowNorm[ARROW_CYL].push_back(z1);
+        s_ArrowTri[ARROW_CYL_CAP].push_back(x0); s_ArrowTri[ARROW_CYL_CAP].push_back(0); s_ArrowTri[ARROW_CYL_CAP].push_back(0);
+        s_ArrowTri[ARROW_CYL_CAP].push_back(x0); s_ArrowTri[ARROW_CYL_CAP].push_back(CYL_RADIUS*y1); s_ArrowTri[ARROW_CYL_CAP].push_back(CYL_RADIUS*z1);
+        s_ArrowTri[ARROW_CYL_CAP].push_back(x0); s_ArrowTri[ARROW_CYL_CAP].push_back(CYL_RADIUS*y0); s_ArrowTri[ARROW_CYL_CAP].push_back(CYL_RADIUS*z0);
+        s_ArrowNorm[ARROW_CYL_CAP].push_back(-1); s_ArrowNorm[ARROW_CYL_CAP].push_back(0); s_ArrowNorm[ARROW_CYL_CAP].push_back(0);
+        s_ArrowNorm[ARROW_CYL_CAP].push_back(-1); s_ArrowNorm[ARROW_CYL_CAP].push_back(0); s_ArrowNorm[ARROW_CYL_CAP].push_back(0);
+        s_ArrowNorm[ARROW_CYL_CAP].push_back(-1); s_ArrowNorm[ARROW_CYL_CAP].push_back(0); s_ArrowNorm[ARROW_CYL_CAP].push_back(0);
+        x0 = ARROW_END-CONE_LENGTH;
+        x1 = ARROW_END;
+        nx = CONE_RADIUS/(x1-x0);
+        nn = 1.0f/sqrtf(nx*nx+1);
+        s_ArrowTri[ARROW_CONE].push_back(x1); s_ArrowTri[ARROW_CONE].push_back(0); s_ArrowTri[ARROW_CONE].push_back(0);
+        s_ArrowTri[ARROW_CONE].push_back(x0); s_ArrowTri[ARROW_CONE].push_back(CONE_RADIUS*y0); s_ArrowTri[ARROW_CONE].push_back(CONE_RADIUS*z0);
+        s_ArrowTri[ARROW_CONE].push_back(x0); s_ArrowTri[ARROW_CONE].push_back(CONE_RADIUS*y1); s_ArrowTri[ARROW_CONE].push_back(CONE_RADIUS*z1);
+        s_ArrowTri[ARROW_CONE].push_back(x1); s_ArrowTri[ARROW_CONE].push_back(0); s_ArrowTri[ARROW_CONE].push_back(0);
+        s_ArrowTri[ARROW_CONE].push_back(x0); s_ArrowTri[ARROW_CONE].push_back(CONE_RADIUS*y1); s_ArrowTri[ARROW_CONE].push_back(CONE_RADIUS*z1);
+        s_ArrowTri[ARROW_CONE].push_back(x1); s_ArrowTri[ARROW_CONE].push_back(0); s_ArrowTri[ARROW_CONE].push_back(0);
+        s_ArrowNorm[ARROW_CONE].push_back(nn*nx); s_ArrowNorm[ARROW_CONE].push_back(nn*y0); s_ArrowNorm[ARROW_CONE].push_back(nn*z0);
+        s_ArrowNorm[ARROW_CONE].push_back(nn*nx); s_ArrowNorm[ARROW_CONE].push_back(nn*y0); s_ArrowNorm[ARROW_CONE].push_back(nn*z0);
+        s_ArrowNorm[ARROW_CONE].push_back(nn*nx); s_ArrowNorm[ARROW_CONE].push_back(nn*y1); s_ArrowNorm[ARROW_CONE].push_back(nn*z1);
+        s_ArrowNorm[ARROW_CONE].push_back(nn*nx); s_ArrowNorm[ARROW_CONE].push_back(nn*y0); s_ArrowNorm[ARROW_CONE].push_back(nn*z0);
+        s_ArrowNorm[ARROW_CONE].push_back(nn*nx); s_ArrowNorm[ARROW_CONE].push_back(nn*y1); s_ArrowNorm[ARROW_CONE].push_back(nn*z1);
+        s_ArrowNorm[ARROW_CONE].push_back(nn*nx); s_ArrowNorm[ARROW_CONE].push_back(nn*y1); s_ArrowNorm[ARROW_CONE].push_back(nn*z1);
+        s_ArrowTri[ARROW_CONE_CAP].push_back(x0); s_ArrowTri[ARROW_CONE_CAP].push_back(0); s_ArrowTri[ARROW_CONE_CAP].push_back(0);
+        s_ArrowTri[ARROW_CONE_CAP].push_back(x0); s_ArrowTri[ARROW_CONE_CAP].push_back(CONE_RADIUS*y1); s_ArrowTri[ARROW_CONE_CAP].push_back(CONE_RADIUS*z1);
+        s_ArrowTri[ARROW_CONE_CAP].push_back(x0); s_ArrowTri[ARROW_CONE_CAP].push_back(CONE_RADIUS*y0); s_ArrowTri[ARROW_CONE_CAP].push_back(CONE_RADIUS*z0);
+        s_ArrowNorm[ARROW_CONE_CAP].push_back(-1); s_ArrowNorm[ARROW_CONE_CAP].push_back(0); s_ArrowNorm[ARROW_CONE_CAP].push_back(0);
+        s_ArrowNorm[ARROW_CONE_CAP].push_back(-1); s_ArrowNorm[ARROW_CONE_CAP].push_back(0); s_ArrowNorm[ARROW_CONE_CAP].push_back(0);
+        s_ArrowNorm[ARROW_CONE_CAP].push_back(-1); s_ArrowNorm[ARROW_CONE_CAP].push_back(0); s_ArrowNorm[ARROW_CONE_CAP].push_back(0);
+    }
+
+    //for(i=0; i<4; ++i)
+    //{
+    //    s_ArrowTriProj[i].clear();
+    //    s_ArrowTriProj[i].resize(2*(s_ArrowTri[i].size()/3), 0);
+    //    s_ArrowColLight[i].clear();
+    //    s_ArrowColLight[i].resize(s_ArrowTri[i].size()/3, 0);
+    //}
+}
+
+////////////////////////////////////////////////////////////////////////////
+// End Copied Straight from AntTweakBar
+////////////////////////////////////////////////////////////////////////////
+
+IGL_INLINE void igl::draw_beach_ball()
+{
+  using namespace std;
+
+  CreateSphere();
+  // Draw triangles
+  glEnable(GL_COLOR_MATERIAL);
+  glColorMaterial(GL_FRONT_AND_BACK,GL_DIFFUSE);
+  float mat_ambient[4] = {0.1,0.1,0.1,1.0};
+  float mat_specular[4] = {0.0,0.0,0.0,1.0};
+  float mat_shininess = 1;
+  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT,  mat_ambient);
+  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, mat_specular);
+  glMaterialf( GL_FRONT_AND_BACK, GL_SHININESS, mat_shininess);
+
+  glPushMatrix();
+  glScalef(0.7,0.7,0.7);
+  glEnable(GL_NORMALIZE);
+  glBegin(GL_TRIANGLES);
+  for(int i = 0;i<(int)s_SphCol.size();i++)
+  {
+    glNormal3fv(&s_SphTri[i*3]);
+    glColor4ub(GLubyte(s_SphCol[i]>>16), GLubyte(s_SphCol[i]>>8), GLubyte(s_SphCol[i]), GLubyte(s_SphCol[i]>>24));
+    glVertex3fv(&s_SphTri[i*3]);
+  }
+  glEnd();
+  glPopMatrix();
+
+  CreateArrow();
+  for(int k = 0;k<3;k++)
+  {
+    glPushMatrix();
+    glColor3f(k==0,k==1,k==2);
+    glRotatef((k==2?-1.0:1.0)*90,k==0,k==2,k==1);
+    glBegin(GL_TRIANGLES);
+    for(int j = 0;j<4;j++)
+    {
+      for(int i = 0;i<(int)s_ArrowTri[j].size();i+=3)
+      {
+        glNormal3fv(&s_ArrowNorm[j][i]);
+        glVertex3fv(&s_ArrowTri[j][i]);
+      }
+    }
+    glEnd();
+    glPopMatrix();
+  }
+}

include/igl/draw_beach_ball.h

@@ -0,0 +1,16 @@
+#ifndef IGL_DRAW_BEACH_BALL_H
+#define IGL_DRAW_BEACH_BALL_H
+#include "igl_inline.h"
+
+namespace igl
+{
+  // Draw a beach ball icon/glyph (from AntTweakBar) at the current origin
+  // according to the current orientation: ball has radius 0.75 and axis have
+  // length 1.15
+  IGL_INLINE void draw_beach_ball();
+  #ifdef IGL_HEADER
+  #  include "draw_beach_ball.cpp"
+  #endif
+}
+
+#endif

include/igl/mat_to_quat.cpp

@@ -0,0 +1,28 @@
+#include "mat_to_quat.h"
+#include <cmath>
+
+// This could be replaced by something fast
+template <typename Q_type>
+static inline Q_type ReciprocalSqrt( const Q_type x )
+{
+  return 1.0/sqrt(x);
+}
+
+// Converts row major order matrix to quat
+// http://software.intel.com/sites/default/files/m/d/4/1/d/8/293748.pdf
+template <typename Q_type>
+IGL_INLINE void igl::mat4_to_quat(const Q_type * m, Q_type * q)
+{
+  Q_type t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f; 
+  Q_type s = ReciprocalSqrt( t ) * 0.5f;
+  q[3] = s * t;
+  q[2] = ( m[0 * 4 + 1] - m[1 * 4 + 0] ) * s; 
+  q[1] = ( m[2 * 4 + 0] - m[0 * 4 + 2] ) * s; 
+  q[0] = ( m[1 * 4 + 2] - m[2 * 4 + 1] ) * s;
+}
+
+
+#ifndef IGL_HEADER_ONLY
+// Explicit template specialization
+template void igl::mat4_to_quat<double>(double const*, double*);
+#endif

include/igl/mat_to_quat.h

@@ -0,0 +1,22 @@
+#ifndef IGL_MAT_TO_QUAT_H
+#define IGL_MAT_TO_QUAT_H
+#include "igl_inline.h"
+namespace igl
+{
+  // Convert a OpenGL (rotation) matrix to a quaternion
+  //
+  // Input:
+  //   m  16-element opengl rotation matrix
+  // Output:
+  //   q  4-element  quaternion (not normalized)
+  template <typename Q_type>
+  IGL_INLINE void mat4_to_quat(const Q_type * m, Q_type * q);
+  // TODO: implement for mat3 etc.
+}
+
+#ifdef IGL_HEADER_ONLY
+#  include "mat_to_quat.cpp"
+#endif
+
+#endif
+

include/igl/trackball.cpp

@@ -35,7 +35,6 @@ IGL_INLINE void igl::trackball(
   const int w,
   const int h,
   const Q_type speed_factor,
-  const Q_type * down_quat,
   const int down_mouse_x,
   const int down_mouse_y,
   const int mouse_x,
@@ -68,34 +67,59 @@ IGL_INLINE void igl::trackball(
     {
       angle *= 1.0 + 0.2f*(sqrt(x*x+y*y)-1.0);
     }
-    double qrot[4], qres[4], qorig[4];
+    double qrot[4];
     axis_angle_to_quat(axis,angle,qrot);
+    quat[0] = qrot[0];
+    quat[1] = qrot[1];
+    quat[2] = qrot[2];
+    quat[3] = qrot[3];
+  }
+}
 
-    double nqorig =
-      sqrt(down_quat[0]*down_quat[0]+
-      down_quat[1]*down_quat[1]+
-      down_quat[2]*down_quat[2]+
-      down_quat[3]*down_quat[3]);
 
-    if( fabs(nqorig)>igl::DOUBLE_EPS_SQ )
-    {
-        qorig[0] = down_quat[0]/nqorig;
-        qorig[1] = down_quat[1]/nqorig;
-        qorig[2] = down_quat[2]/nqorig;
-        qorig[3] = down_quat[3]/nqorig;
-        igl::quat_mult<double>(qrot,qorig,qres);
-        quat[0] = qres[0];
-        quat[1] = qres[1];
-        quat[2] = qres[2];
-        quat[3] = qres[3];
-    }
-    else
-    {
-        quat[0] = qrot[0];
-        quat[1] = qrot[1];
-        quat[2] = qrot[2];
-        quat[3] = qrot[3];
-    }
+template <typename Q_type>
+IGL_INLINE void igl::trackball(
+  const int w,
+  const int h,
+  const Q_type speed_factor,
+  const Q_type * down_quat,
+  const int down_mouse_x,
+  const int down_mouse_y,
+  const int mouse_x,
+  const int mouse_y,
+  Q_type * quat)
+{
+  double qrot[4], qres[4], qorig[4];
+  igl::trackball<double>(
+    w,h,
+    speed_factor,
+    down_mouse_x,down_mouse_y,
+    mouse_x,mouse_y,
+    qrot);
+  double nqorig =
+    sqrt(down_quat[0]*down_quat[0]+
+    down_quat[1]*down_quat[1]+
+    down_quat[2]*down_quat[2]+
+    down_quat[3]*down_quat[3]);
+
+  if( fabs(nqorig)>igl::DOUBLE_EPS_SQ )
+  {
+      qorig[0] = down_quat[0]/nqorig;
+      qorig[1] = down_quat[1]/nqorig;
+      qorig[2] = down_quat[2]/nqorig;
+      qorig[3] = down_quat[3]/nqorig;
+      igl::quat_mult<double>(qrot,qorig,qres);
+      quat[0] = qres[0];
+      quat[1] = qres[1];
+      quat[2] = qres[2];
+      quat[3] = qres[3];
+  }
+  else
+  {
+      quat[0] = qrot[0];
+      quat[1] = qrot[1];
+      quat[2] = qrot[2];
+      quat[3] = qrot[3];
   }
 }
 

include/igl/trackball.h

@@ -4,6 +4,28 @@
 
 namespace igl
 {
+  // Applies a trackball drag to identity
+  // Inputs:
+  //   w  width of the trackball context
+  //   h  height of the trackball context
+  //   speed_factor  controls how fast the trackball feels, 1 is normal
+  //   down_mouse_x  x position of mouse down
+  //   down_mouse_y  y position of mouse down
+  //   mouse_x  current x position of mouse
+  //   mouse_y  current y position of mouse
+  // Outputs:
+  //   quat  the resulting rotation (as quaternion)
+  template <typename Q_type>
+  IGL_INLINE void trackball(
+    const int w,
+    const int h,
+    const Q_type speed_factor,
+    const int down_mouse_x,
+    const int down_mouse_y,
+    const int mouse_x,
+    const int mouse_y,
+    Q_type * quat);
+
   // Applies a trackball drag to a given rotation
   // Inputs:
   //   w  width of the trackball context