added a bunch of quaternion math, vector math and trackball

Former-commit-id: d28a49e14afebfb0150b7d5f5ed9f825c699ba37

EPS.h

@@ -0,0 +1,9 @@
+#ifndef EPS_H
+#define EPS_H
+// Define a standard value for double epsilon
+namespace igl
+{
+  const double DOUBLE_EPS    = 1.0e-14;
+  const double DOUBLE_EPS_SQ = 1.0e-28;
+}
+#endif

PI.h

@@ -1,4 +1,9 @@
 namespace igl
 {
+  // Use standard mathematical constants' M_PI if available
+#ifdef M_PI
+  const double PI = M_PI;
+#else
   const double PI = 3.1415926535897932384626433832795;
+#endif
 }

axis_angle_to_quat.h

@@ -0,0 +1,41 @@
+#include <EPS.h>
+namespace igl
+{
+  // Convert axis angle representation of a rotation to a quaternion
+  // A Quaternion, q, is defined here as an arrays of four scalars (x,y,z,w),
+  // such that q = x*i + y*j + z*k + w
+  // Inputs:
+  //   axis  3d vector
+  //   angle  scalar
+  // Outputs:
+  //   quaternion
+  inline void axis_angle_to_quat(
+    const double *axis, 
+    const double angle,
+    double *out);
+}
+
+// Implementation
+
+// http://www.antisphere.com/Wiki/tools:anttweakbar
+inline void igl::axis_angle_to_quat(
+  const double *axis, 
+  const double angle,
+  double *out)
+{
+    double n = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
+    if( fabs(n)>igl::DOUBLE_EPS )
+    {
+        double f = 0.5*angle;
+        out[3] = cos(f);
+        f = sin(f)/sqrt(n);
+        out[0] = axis[0]*f;
+        out[1] = axis[1]*f;
+        out[2] = axis[2]*f;
+    }
+    else
+    {
+        out[3] = 1.0;
+        out[0] = out[1] = out[2] = 0.0;
+    }
+}

canonical_quaternions.h

@@ -0,0 +1,99 @@
+// Define some canonical quaternions for floats and doubles
+// A Quaternion, q, is defined here as an arrays of four scalars (x,y,z,w),
+// such that q = x*i + y*j + z*k + w
+namespace igl
+{
+#  define SQRT_2_OVER_2 0.707106781f
+
+  // Float versions
+  // Identity
+  const float IDENTITY_QUAT_F[4] = {0,0,0,1};
+  // The following match the Matlab canonical views
+  // X point right, Y pointing up and Z point out
+  const float XY_PLANE_QUAT_F[4] = {0,0,0,1};
+  // X points right, Y points *in* and Z points up
+  const float XZ_PLANE_QUAT_F[4] = {-SQRT_2_OVER_2,0,0,SQRT_2_OVER_2};
+  // X points out, Y points right, and Z points up
+  const float YZ_PLANE_QUAT_F[4] = {-0.5,-0.5,-0.5,0.5};
+  const float CANONICAL_VIEW_QUAT_F[][4] = 
+    {
+      {             0,             0,             0,             1},
+      {             0,             0, SQRT_2_OVER_2, SQRT_2_OVER_2},
+      {             0,             0,             1,             0},
+      {             0,             0, SQRT_2_OVER_2,-SQRT_2_OVER_2},
+  
+      {             0,            -1,             0,             0},
+      {-SQRT_2_OVER_2, SQRT_2_OVER_2,             0,             0},
+      {            -1,             0,             0,             0},
+      {-SQRT_2_OVER_2,-SQRT_2_OVER_2,             0,             0},
+  
+      {          -0.5,          -0.5,          -0.5,           0.5},
+      {             0,-SQRT_2_OVER_2,             0, SQRT_2_OVER_2},
+      {           0.5,          -0.5,           0.5,           0.5},
+      { SQRT_2_OVER_2,             0, SQRT_2_OVER_2,             0},
+  
+      { SQRT_2_OVER_2,             0,-SQRT_2_OVER_2,             0},
+      {           0.5,           0.5,          -0.5,           0.5},
+      {             0, SQRT_2_OVER_2,             0, SQRT_2_OVER_2},
+      {          -0.5,           0.5,           0.5,           0.5},
+  
+      {             0, SQRT_2_OVER_2, SQRT_2_OVER_2,             0},
+      {          -0.5,           0.5,           0.5,          -0.5},
+      {-SQRT_2_OVER_2,             0,             0,-SQRT_2_OVER_2},
+      {          -0.5,          -0.5,          -0.5,          -0.5},
+  
+      {-SQRT_2_OVER_2,             0,             0, SQRT_2_OVER_2},
+      {          -0.5,          -0.5,           0.5,           0.5},
+      {             0,-SQRT_2_OVER_2, SQRT_2_OVER_2,             0},
+      {           0.5,          -0.5,           0.5,          -0.5}
+    };
+  const size_t NUM_CANONICAL_VIEW_QUAT_F = 24;
+#  undef SQRT_2_OVER_2
+
+#  define SQRT_2_OVER_2 0.707106781186548f
+  // Double versions
+  // Identity
+  const double IDENTITY_QUAT_D[4] = {0,0,0,1};
+  // The following match the Matlab canonical views
+  // X point right, Y pointing up and Z point out
+  const double XY_PLANE_QUAT_D[4] = {0,0,0,1};
+  // X points right, Y points *in* and Z points up
+  const double XZ_PLANE_QUAT_D[4] = {-SQRT_2_OVER_2,0,0,SQRT_2_OVER_2};
+  // X points out, Y points right, and Z points up
+  const double YZ_PLANE_QUAT_D[4] = {-0.5,-0.5,-0.5,0.5};
+  const double CANONICAL_VIEW_QUAT_D[][4] = 
+    {
+      {             0,             0,             0,             1},
+      {             0,             0, SQRT_2_OVER_2, SQRT_2_OVER_2},
+      {             0,             0,             1,             0},
+      {             0,             0, SQRT_2_OVER_2,-SQRT_2_OVER_2},
+  
+      {             0,            -1,             0,             0},
+      {-SQRT_2_OVER_2, SQRT_2_OVER_2,             0,             0},
+      {            -1,             0,             0,             0},
+      {-SQRT_2_OVER_2,-SQRT_2_OVER_2,             0,             0},
+  
+      {          -0.5,          -0.5,          -0.5,           0.5},
+      {             0,-SQRT_2_OVER_2,             0, SQRT_2_OVER_2},
+      {           0.5,          -0.5,           0.5,           0.5},
+      { SQRT_2_OVER_2,             0, SQRT_2_OVER_2,             0},
+  
+      { SQRT_2_OVER_2,             0,-SQRT_2_OVER_2,             0},
+      {           0.5,           0.5,          -0.5,           0.5},
+      {             0, SQRT_2_OVER_2,             0, SQRT_2_OVER_2},
+      {          -0.5,           0.5,           0.5,           0.5},
+  
+      {             0, SQRT_2_OVER_2, SQRT_2_OVER_2,             0},
+      {          -0.5,           0.5,           0.5,          -0.5},
+      {-SQRT_2_OVER_2,             0,             0,-SQRT_2_OVER_2},
+      {          -0.5,          -0.5,          -0.5,          -0.5},
+  
+      {-SQRT_2_OVER_2,             0,             0, SQRT_2_OVER_2},
+      {          -0.5,          -0.5,           0.5,           0.5},
+      {             0,-SQRT_2_OVER_2, SQRT_2_OVER_2,             0},
+      {           0.5,          -0.5,           0.5,          -0.5}
+    };
+  const size_t NUM_CANONICAL_VIEW_QUAT_D = 24;
+
+#  undef SQRT_2_OVER_2
+}

cross.h

@@ -0,0 +1,28 @@
+#ifndef IGL_CROSS_H
+#define IGL_CROSS_H
+namespace igl
+{
+  // Computes out = cross(a,b)
+  // Inputs:
+  //   a  left 3d vector
+  //   b  right 3d vector
+  // Outputs:
+  //   out  result 3d vector
+  inline void cross(
+    const double *a, 
+    const double *b,
+    double *out);
+}
+
+// Implementation
+// http://www.antisphere.com/Wiki/tools:anttweakbar
+inline void igl::cross(
+  const double *a, 
+  const double *b,
+  double *out)
+{
+  out[0] = a[1]*b[2]-a[2]*b[1];
+  out[1] = a[2]*b[0]-a[0]*b[2];
+  out[2] = a[0]*b[1]-a[1]*b[0];
+}
+#endif

dot.h

@@ -0,0 +1,23 @@
+#ifndef IGL_DOT_H
+#define IGL_DOT_H
+namespace igl
+{
+  // Computes out = dot(a,b)
+  // Inputs:
+  //   a  left 3d vector
+  //   b  right 3d vector
+  // Returns scalar dot product
+  inline double dot(
+    const double *a, 
+    const double *b);
+}
+
+// Implementation
+// http://www.antisphere.com/Wiki/tools:anttweakbar
+inline double igl::dot(
+  const double *a, 
+  const double *b)
+{
+  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
+}
+#endif

quat_mult.h

@@ -0,0 +1,28 @@
+namespace igl
+{
+  // Computes out = q1 * q2 with quaternion multiplication
+  // A Quaternion, q, is defined here as an arrays of four scalars (x,y,z,w),
+  // such that q = x*i + y*j + z*k + w
+  // Inputs:
+  //   q1  left quaternion
+  //   q2  right quaternion
+  // Outputs:
+  //   out  result of multiplication
+  inline void quat_mult(
+    const double *q1, 
+    const double *q2,
+    double *out);
+};
+
+// Implementation
+// http://www.antisphere.com/Wiki/tools:anttweakbar
+inline void igl::quat_mult(
+  const double *q1, 
+  const double *q2,
+  double *out)
+{
+    out[0] = q1[3]*q2[0] + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1];
+    out[1] = q1[3]*q2[1] + q1[1]*q2[3] + q1[2]*q2[0] - q1[0]*q2[2];
+    out[2] = q1[3]*q2[2] + q1[2]*q2[3] + q1[0]*q2[1] - q1[1]*q2[0];
+    out[3] = q1[3]*q2[3] - (q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2]);
+}

quat_to_mat.h

@@ -0,0 +1,42 @@
+// Name history:
+//   quat2mat  until 16 Sept 2011
+namespace igl
+{
+  // Convert a quaternion to a 4x4 matrix
+  // A Quaternion, q, is defined here as an arrays of four scalars (x,y,z,w),
+  // such that q = x*i + y*j + z*k + w
+  // Input:
+  //   quat  pointer to four elements of quaternion (x,y,z,w)  
+  // Output:
+  //   mat  pointer to 16 elements of matrix
+  void quat_to_mat(const float * quat, float * mat);
+}
+
+// Implementation
+
+void igl::quat_to_mat(const float * quat, float * mat)
+{
+  float yy2 = 2.0f * quat[1] * quat[1];
+  float xy2 = 2.0f * quat[0] * quat[1];
+  float xz2 = 2.0f * quat[0] * quat[2];
+  float yz2 = 2.0f * quat[1] * quat[2];
+  float zz2 = 2.0f * quat[2] * quat[2];
+  float wz2 = 2.0f * quat[3] * quat[2];
+  float wy2 = 2.0f * quat[3] * quat[1];
+  float wx2 = 2.0f * quat[3] * quat[0];
+  float xx2 = 2.0f * quat[0] * quat[0];
+  mat[0*4+0] = - yy2 - zz2 + 1.0f;
+  mat[0*4+1] = xy2 + wz2;
+  mat[0*4+2] = xz2 - wy2;
+  mat[0*4+3] = 0;
+  mat[1*4+0] = xy2 - wz2;
+  mat[1*4+1] = - xx2 - zz2 + 1.0f;
+  mat[1*4+2] = yz2 + wx2;
+  mat[1*4+3] = 0;
+  mat[2*4+0] = xz2 + wy2;
+  mat[2*4+1] = yz2 - wx2;
+  mat[2*4+2] = - xx2 - yy2 + 1.0f;
+  mat[2*4+3] = 0;
+  mat[3*4+0] = mat[3*4+1] = mat[3*4+2] = 0;
+  mat[3*4+3] = 1;
+}

trackball.h

@@ -0,0 +1,107 @@
+#include <dot.h>
+#include <cross.h>
+#include <axis_angle_to_quat.h>
+#include <quat_mult.h>
+
+namespace igl
+{
+  // Applies a trackball drag to a given rotation
+  // 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_quat  rotation at mouse down, i.e. the rotation we're applying the
+  //     trackball motion to (as quaternion)
+  //   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)
+  void trackball(
+    const int w,
+    const int h,
+    const double speed_factor,
+    const float * down_quat,
+    const int down_mouse_x,
+    const int down_mouse_y,
+    const int mouse_x,
+    const int mouse_y,
+    float * quat);
+}
+
+// Utility inline functions
+inline float _QuatD(int w, int h)
+{
+    return (float)min(abs(w), abs(h)) - 4;
+}
+inline float _QuatIX(int x, int w, int h)
+{
+    return (2.0f*(float)x - (float)w - 1.0f)/_QuatD(w, h);
+}
+inline float _QuatIY(int y, int w, int h)
+{
+    return (-2.0f*(float)y + (float)h - 1.0f)/_QuatD(w, h);
+}
+
+void igl::trackball(
+  const int w,
+  const int h,
+  const double speed_factor,
+  const float * down_quat,
+  const int down_mouse_x,
+  const int down_mouse_y,
+  const int mouse_x,
+  const int mouse_y,
+  float * quat)
+{
+  double original_x = 
+    _QuatIX(speed_factor*(down_mouse_x-w/2)+w/2, w, h);
+  double original_y = 
+    _QuatIY(speed_factor*(down_mouse_y-h/2)+h/2, w, h);
+
+  double x = _QuatIX(speed_factor*(mouse_x-w/2)+w/2, w, h);
+  double y = _QuatIY(speed_factor*(mouse_y-h/2)+h/2, w, h);
+
+  double z = 1;
+  double n0 = sqrt(original_x*original_x + original_y*original_y + z*z);
+  double n1 = sqrt(x*x + y*y + z*z);
+  if(n0>DOUBLE_EPS && n1>DOUBLE_EPS)
+  {
+    double v0[] = { original_x/n0, original_y/n0, z/n0 };
+    double v1[] = { x/n1, y/n1, z/n1 };
+    double axis[3];
+    cross(v0,v1,axis);
+    double sa = sqrt(dot(axis, axis));
+    double ca = dot(v0, v1);
+    double angle = atan2(sa, ca);
+    if( x*x+y*y>1.0 )
+    {
+      angle *= 1.0 + 0.2f*(sqrt(x*x+y*y)-1.0);
+    }
+    double qrot[4], qres[4], qorig[4];
+    axis_angle_to_quat(axis,angle,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)>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;
+        quat_mult(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];
+    }
+  }
+}