Templated the quaternion functions and EPS, a few more quaternion functions, fixed prototypes and header defines in writeOBJ

Former-commit-id: 4e977cb3fe7b29d3675fa5cbfbb833d93817cd8e

EPS.h

@@ -7,5 +7,21 @@ namespace igl
   const double DOUBLE_EPS_SQ = 1.0e-28;
   const float FLOAT_EPS    = 1.0e-7;
   const float FLOAT_EPS_SQ = 1.0e-14;
+  // Function returning EPS for corresponding type
+  template <typename S_type> inline S_type EPS();
+  // Template specializations for float and double
+  template <> inline float EPS<float>();
+  template <> inline double EPS<double>();
+}
+
+// Implementation
+template <> inline float igl::EPS()
+{
+  return igl::FLOAT_EPS;
+}
+
+template <> inline double igl::EPS()
+{
+  return igl::DOUBLE_EPS;
 }
 #endif

axis_angle_to_quat.h

@@ -13,52 +13,25 @@ namespace igl
   //   angle  scalar
   // Outputs:
   //   quaternion
+  template <typename Q_type>
   inline void axis_angle_to_quat(
-    const double *axis, 
-    const double angle,
-    double *out);
-  // Same but with floats
-  inline void axis_angle_to_quat(
-    const float *axis, 
-    const float angle,
-    float *out);
+    const Q_type *axis, 
+    const Q_type angle,
+    Q_type *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;
-    }
-}
-
 // http://www.antisphere.com/Wiki/tools:anttweakbar
+template <typename Q_type>
 inline void igl::axis_angle_to_quat(
-  const float *axis, 
-  const float angle,
-  float *out)
+  const Q_type *axis, 
+  const Q_type angle,
+  Q_type *out)
 {
-    float n = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
-    if( fabs(n)>igl::FLOAT_EPS )
+    Q_type n = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
+    if( fabs(n)>igl::EPS<Q_type>())
     {
-        float f = 0.5*angle;
+        Q_type f = 0.5*angle;
         out[3] = cos(f);
         f = sin(f)/sqrt(n);
         out[0] = axis[0]*f;

create_vector_vbo.h

@@ -26,12 +26,16 @@ namespace igl
 }
 
 // Implementation
+#include <cassert>
 
 // http://www.songho.ca/opengl/gl_vbo.html#create
 void igl::create_vector_vbo(
   const Eigen::MatrixXd & V,
   GLuint & V_vbo_id)
 {
+  // Expects that input is list of 3D vectors along rows
+  assert(V.cols() == 3);
+
   // Generate Buffers
   glGenBuffersARB(1,&V_vbo_id);
   // Bind Buffers

get_seconds.h

@@ -1,16 +1,20 @@
+#ifndef IGL_GET_SECONDS_H
+#define IGL_GET_SECONDS_H
 #include <sys/time.h>
 
 namespace igl
 {
   // Return the current time since Epoch in seconds
-  double get_seconds();
+  inline double get_seconds();
+
 }
 
 //Implementation
 
-double igl::get_seconds()
+inline double igl::get_seconds()
 {
   timeval time;
   gettimeofday(&time, NULL);
   return time.tv_sec + time.tv_usec / 1e6;
 }
+#endif

normalize_quat.h

@@ -0,0 +1,47 @@
+#ifndef IGL_NORMALIZE_QUAT_H
+#define IGL_NORMALIZE_QUAT_H
+
+namespace igl
+{
+  // Normalize 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:
+  //   q  input quaternion
+  // Outputs:
+  //   out  result of normalization, allowed to be same as q
+  // Returns true on success, false if len(q) < EPS
+  template <typename Q_type>
+  inline bool normalize_quat(
+    const Q_type *q,
+    Q_type *out);
+};
+
+// Implementation
+#include "EPS.h"
+
+template <typename Q_type>
+inline bool igl::normalize_quat(
+  const Q_type *q,
+  Q_type *out)
+{
+  // Get length
+  Q_type len = sqrt(
+    q[0]*q[0]+
+    q[1]*q[1]+
+    q[2]*q[2]+
+    q[3]*q[3]);
+
+  // Noramlize each coordinate
+  out[0] = q[0]/len;
+  out[1] = q[1]/len;
+  out[2] = q[2]/len;
+  out[3] = q[3]/len;
+
+  // Test whether length was below Epsilon
+  return (len > igl::EPS<Q_type>());
+}
+
+#endif
+
+

normalize_rows.h

@@ -15,6 +15,9 @@ namespace igl
 
 void igl::normalize_rows(const Eigen::MatrixXd & A, Eigen::MatrixXd & B)
 {
+  // Resize output
+  B.resize(A.rows(),A.cols());
+
   // loop over rows
   for(int i = 0; i < A.rows();i++)
   {

per_face_normals.h

@@ -21,7 +21,7 @@ void igl::per_face_normals(
   const Eigen::MatrixXi & F,
   Eigen::MatrixXd & N)
 {
-  N = Eigen::MatrixXd(F.rows(),3);
+  N.resize(F.rows(),3);
   // loop over faces
   for(int i = 0; i < F.rows();i++)
   {

per_vertex_normals.h

@@ -1,6 +1,10 @@
 #ifndef IGL_PER_VERTEX_NORMALS_H
 #define IGL_PER_VERTEX_NORMALS_H
 #include <Eigen/Core>
+// Note: So for this only computes normals per vertex as uniformly weighted
+// averages of incident triangle normals. It would be nice to support more or
+// all of the methods here:
+// "A comparison of algorithms for vertex normal computation"
 namespace igl
 {
   // Compute vertex normals via vertex position list, face list
@@ -27,7 +31,8 @@ void igl::per_vertex_normals(
   Eigen::MatrixXd PFN;
   igl::per_face_normals(V,F,PFN);
 
-  N = Eigen::MatrixXd(V.rows(),3);
+  // Resize for output
+  N.resize(V.rows(),3);
   // loop over vertices, setting normalize to 0
   for(int i = 0; i < N.rows();i++)
   {

quat_conjugate.h

@@ -0,0 +1,33 @@
+#ifndef IGL_QUAT_CONJUGATE_H
+#define IGL_QUAT_CONJUGATE_H
+
+namespace igl
+{
+  // Compute conjugate of given quaternion
+  // http://en.wikipedia.org/wiki/Quaternion#Conjugation.2C_the_norm.2C_and_reciprocal
+  // 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  input quaternion
+  // Outputs:
+  //   out  result of conjugation, allowed to be same as input
+  template <typename Q_type>
+  inline void quat_conjugate(
+    const Q_type *q1, 
+    Q_type *out);
+};
+
+// Implementation
+template <typename Q_type>
+inline void igl::quat_conjugate(
+  const Q_type *q1, 
+  Q_type *out)
+{
+  out[0] = -q1[0];
+  out[1] = -q1[1];
+  out[2] = -q1[2];
+  out[3] = q1[3];
+}
+
+#endif
+

quat_mult.h

@@ -11,23 +11,30 @@ namespace igl
   //   q2  right quaternion
   // Outputs:
   //   out  result of multiplication
+  template <typename Q_type>
   inline void quat_mult(
-    const double *q1, 
-    const double *q2,
-    double *out);
+    const Q_type *q1, 
+    const Q_type *q2,
+    Q_type *out);
 };
 
 // Implementation
+#include <cassert>
 // http://www.antisphere.com/Wiki/tools:anttweakbar
+template <typename Q_type>
 inline void igl::quat_mult(
-  const double *q1, 
-  const double *q2,
-  double *out)
+  const Q_type *q1, 
+  const Q_type *q2,
+  Q_type *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]);
+  // output can't be either of the inputs
+  assert(q1 != out);
+  assert(q2 != 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]);
 }
 
 #endif

quat_to_mat.h

@@ -9,22 +9,24 @@ namespace igl
   //   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);
+  template <typename Q_type>
+  void quat_to_mat(const Q_type * quat, Q_type * mat);
 }
 
 // Implementation
 
-void igl::quat_to_mat(const float * quat, float * mat)
+template <typename Q_type>
+void igl::quat_to_mat(const Q_type * quat, Q_type * 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];
+  Q_type yy2 = 2.0f * quat[1] * quat[1];
+  Q_type xy2 = 2.0f * quat[0] * quat[1];
+  Q_type xz2 = 2.0f * quat[0] * quat[2];
+  Q_type yz2 = 2.0f * quat[1] * quat[2];
+  Q_type zz2 = 2.0f * quat[2] * quat[2];
+  Q_type wz2 = 2.0f * quat[3] * quat[2];
+  Q_type wy2 = 2.0f * quat[3] * quat[1];
+  Q_type wx2 = 2.0f * quat[3] * quat[0];
+  Q_type 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;

rotate_by_quat.h

@@ -0,0 +1,56 @@
+#ifndef IGL_ROTATE_BY_QUAT_H
+#define IGL_ROTATE_BY_QUAT_H
+
+namespace igl
+{
+  // Compute rotation of a given vector/point by 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:
+  //   v  input 3d point/vector
+  //   q  input quaternion
+  // Outputs:
+  //   out  result of rotation, allowed to be same as v
+  template <typename Q_type>
+  inline void rotate_by_quat(
+    const Q_type *v,
+    const Q_type *q, 
+    Q_type *out);
+};
+
+// Implementation
+#include "quat_conjugate.h"
+#include "quat_mult.h"
+#include "normalize_quat.h"
+#include <cassert>
+
+template <typename Q_type>
+inline void igl::rotate_by_quat(
+  const Q_type *v,
+  const Q_type *q,
+  Q_type *out)
+{
+  // Quaternion form of v, copy data in v, (as a result out can be same pointer
+  // as v)
+  Q_type quat_v[4] = {v[0],v[1],v[2],0};
+
+  // normalize input 
+  Q_type normalized_q[4];
+  bool normalized = igl::normalize_quat<Q_type>(q,normalized_q);
+  assert(normalized);
+
+  // Conjugate of q
+  Q_type q_conj[4];
+  igl::quat_conjugate<Q_type>(normalized_q,q_conj);
+
+  // Rotate of vector v by quaternion q is:
+  // q*v*conj(q)
+  // Compute q*v
+  Q_type q_mult_quat_v[4];
+  igl::quat_mult<Q_type>(normalized_q,quat_v,q_mult_quat_v);
+  // Compute (q*v) * conj(q)
+  igl::quat_mult<Q_type>(q_mult_quat_v,q_conj,out);
+}
+
+#endif
+

trackball.h

@@ -14,16 +14,17 @@ namespace igl
   //   mouse_y  current y position of mouse
   // Outputs:
   //   quat  the resulting rotation (as quaternion)
+  template <typename Q_type>
   void trackball(
     const int w,
     const int h,
-    const double speed_factor,
-    const float * down_quat,
+    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,
-    float * quat);
+    Q_type * quat);
 }
 
 // Implementation
@@ -37,40 +38,46 @@ namespace igl
 #include <algorithm>
 
 // Utility inline functions
-static inline float _QuatD(int w, int h)
+template <typename Q_type>
+static inline Q_type _QuatD(int w, int h)
 {
-    return (float)std::min(abs(w), abs(h)) - 4;
+  return (Q_type)std::min(abs(w), abs(h)) - 4;
 }
-static inline float _QuatIX(int x, int w, int h)
+template <typename Q_type>
+static inline Q_type _QuatIX(int x, int w, int h)
 {
-    return (2.0f*(float)x - (float)w - 1.0f)/_QuatD(w, h);
+  return (2.0f*(Q_type)x - (Q_type)w - 1.0f)/_QuatD<Q_type>(w, h);
 }
-static inline float _QuatIY(int y, int w, int h)
+template <typename Q_type>
+static inline Q_type _QuatIY(int y, int w, int h)
 {
-    return (-2.0f*(float)y + (float)h - 1.0f)/_QuatD(w, h);
+  return (-2.0f*(Q_type)y + (Q_type)h - 1.0f)/_QuatD<Q_type>(w, h);
 }
 
 // This is largely the trackball as implemented in AntTweakbar. Much of the
 // code is straight from its source in TwMgr.cpp
 // http://www.antisphere.com/Wiki/tools:anttweakbar
+template <typename Q_type>
 void igl::trackball(
   const int w,
   const int h,
-  const double speed_factor,
-  const float * down_quat,
+  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,
-  float * quat)
+  Q_type * quat)
 {
+  assert(speed_factor > 0);
+
   double original_x = 
-    _QuatIX(speed_factor*(down_mouse_x-w/2)+w/2, w, h);
+    _QuatIX<Q_type>(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);
+    _QuatIY<Q_type>(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 x = _QuatIX<Q_type>(speed_factor*(mouse_x-w/2)+w/2, w, h);
+  double y = _QuatIY<Q_type>(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);
@@ -103,7 +110,7 @@ void igl::trackball(
         qorig[1] = down_quat[1]/nqorig;
         qorig[2] = down_quat[2]/nqorig;
         qorig[3] = down_quat[3]/nqorig;
-        quat_mult(qrot,qorig,qres);
+        igl::quat_mult<double>(qrot,qorig,qres);
         quat[0] = qres[0];
         quat[1] = qres[1];
         quat[2] = qres[2];

writeOBJ.h

@@ -3,28 +3,48 @@
 //
 //  Copyright 2011, Daniele Panozzo. All rights reserved.
 
-#ifndef WRITEOBJ_H
-#define WRITEOBJ_H
+// History:
+//  return type changed from void to bool  Alec 20 Sept 2011
+
+#ifndef IGL_WRITEOBJ_H
+#define IGL_WRITEOBJ_H
 
 #include <Eigen/Core>
 #include <string>
-#include <iostream>
-#include <fstream>
 
 namespace igl 
 {
-    // Write a mesh in an ascii obj file
-    void writeOBJ(std::string str, Eigen::MatrixXd& V, Eigen::MatrixXi& F)
-    {
-        std::ofstream s(str.c_str());
-        for(int i=0;i<V.rows();++i)
-            s << "v " << V(i,0) << " " << V(i,1) << " " << V(i,2) << std::endl;
-        
-        for(int i=0;i<F.rows();++i)
-            s << "f " << F(i,0)+1 << " " << F(i,1)+1 << " " << F(i,2)+1 << std::endl;
-        
-        s.close();
-    }
+  // Write a mesh in an ascii obj file
+  // Inputs:
+  //   str  path to outputfile
+  //   V  eigen double matrix #V by 3 (mesh vertices)
+  //   F  eigen int matrix #F by 3 (mesh indices)
+  // Returns true on success, false on error
+  bool writeOBJ(const std::string str, const Eigen::MatrixXd& V, const Eigen::MatrixXi& F);
 }
 
-#endif
+// Implementation
+#include <iostream>
+#include <fstream>
+#include <cstdio>
+
+bool igl::writeOBJ(const std::string str, const Eigen::MatrixXd& V, const Eigen::MatrixXi& F)
+{
+  std::ofstream s(str.c_str());
+
+  if(!s.is_open())
+  {
+    fprintf(stderr,"IOError: writeOBJ() could not open %s\n",str.c_str());
+    return false;
+  }
+
+  for(int i=0;i<V.rows();++i)
+      s << "v " << V(i,0) << " " << V(i,1) << " " << V(i,2) << std::endl;
+  
+  for(int i=0;i<F.rows();++i)
+      s << "f " << F(i,0)+1 << " " << F(i,1)+1 << " " << F(i,2)+1 << std::endl;
+  
+  s.close();
+  return true;
+}
+#endif