quiter qp solves, mat3_to_quat, more matlabworkspace saves, polar svd

Former-commit-id: 934cb3d693ede6ba86195cec47359dd28fa4abd2

include/igl/active_set.cpp

@@ -36,7 +36,7 @@ IGL_INLINE igl::SolverStatus igl::active_set(
   Eigen::PlainObjectBase<DerivedZ> & Z
   )
 {
-#define ACTIVE_SET_CPP_DEBUG
+//#define ACTIVE_SET_CPP_DEBUG
   using namespace igl;
   using namespace Eigen;
   using namespace std;

include/igl/mat_max.h

@@ -17,9 +17,9 @@ namespace igl
   //   X  m by n matrix
   //   dim  dimension along which to take max
   // Outputs:
-  //   Y  n-long sparse vector (if dim == 1) 
+  //   Y  n-long vector (if dim == 1) 
   //   or
-  //   Y  m-long sparse vector (if dim == 2)
+  //   Y  m-long vector (if dim == 2)
   //   I  vector the same size as Y containing the indices along dim of maximum
   //     entries
   template <typename T>

include/igl/mat_to_quat.cpp

@@ -73,10 +73,62 @@ IGL_INLINE void igl::mat4_to_quat(const Q_type * mat, Q_type * q)
   //jq.t[2] = mat[2 * 4 + 3];
 }
 
+template <typename Q_type>
+IGL_INLINE void igl::mat3_to_quat(const Q_type * mat, Q_type * q)
+{
+  Q_type trace;
+  Q_type s;
+  Q_type t;
+  int i;
+  int j;
+  int k;
+  
+  static int next[3] = { 1, 2, 0 };
+
+  trace = mat[0 * 3 + 0] + mat[1 * 3 + 1] + mat[2 * 3 + 2];
+
+  if ( trace > 0.0f ) {
+
+    t = trace + 1.0f;
+    s = ReciprocalSqrt( t ) * 0.5f;
+
+    q[3] = s * t;
+    q[0] = ( mat[1 * 3 + 2] - mat[2 * 3 + 1] ) * s;
+    q[1] = ( mat[2 * 3 + 0] - mat[0 * 3 + 2] ) * s;
+    q[2] = ( mat[0 * 3 + 1] - mat[1 * 3 + 0] ) * s;
+
+  } else {
+
+    i = 0;
+    if ( mat[1 * 3 + 1] > mat[0 * 3 + 0] ) {
+      i = 1;
+    }
+    if ( mat[2 * 3 + 2] > mat[i * 3 + i] ) {
+      i = 2;
+    }
+    j = next[i];
+    k = next[j];
+
+    t = ( mat[i * 3 + i] - ( mat[j * 3 + j] + mat[k * 3 + k] ) ) + 1.0f;
+    s = ReciprocalSqrt( t ) * 0.5f;
+
+    q[i] = s * t;
+    q[3] = ( mat[j * 3 + k] - mat[k * 3 + j] ) * s;
+    q[j] = ( mat[i * 3 + j] + mat[j * 3 + i] ) * s;
+    q[k] = ( mat[i * 3 + k] + mat[k * 3 + i] ) * s;
+  }
+
+  //// Unused translation
+  //jq.t[0] = mat[0 * 4 + 3];
+  //jq.t[1] = mat[1 * 4 + 3];
+  //jq.t[2] = mat[2 * 4 + 3];
+}
+
 
 
 #ifndef IGL_HEADER_ONLY
 // Explicit template specialization
 template void igl::mat4_to_quat<double>(double const*, double*);
 template void igl::mat4_to_quat<float>(float const*, float*);
+template void igl::mat3_to_quat<double>(double const*, double*);
 #endif

include/igl/mat_to_quat.h

@@ -11,7 +11,10 @@ namespace igl
   //   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.
+  // Input:
+  //   m  9-element opengl rotation matrix
+  template <typename Q_type>
+  IGL_INLINE void mat3_to_quat(const Q_type * m, Q_type * q);
 }
 
 #ifdef IGL_HEADER_ONLY

include/igl/matlab/MatlabWorkspace.h

@@ -154,7 +154,9 @@ namespace igl
 #include <algorithm>
 #include <vector>
 
-inline igl::MatlabWorkspace::MatlabWorkspace()
+inline igl::MatlabWorkspace::MatlabWorkspace():
+  names(),
+  data()
 {
 }
 

include/igl/min_quad_with_fixed.cpp

@@ -28,7 +28,7 @@ IGL_INLINE bool igl::min_quad_with_fixed_precompute(
   min_quad_with_fixed_data<T> & data
   )
 {
-#define MIN_QUAD_WITH_FIXED_CPP_DEBUG
+//#define MIN_QUAD_WITH_FIXED_CPP_DEBUG
   using namespace Eigen;
   using namespace std;
   using namespace igl;

include/igl/mosek/Makefile

@@ -6,7 +6,7 @@ debug: libiglmosek
 
 include ../../../Makefile.conf
 all: OPTFLAGS += -O3 -DNDEBUG $(OPENMP)
-debug: OPTFLAGS += -g -Wall -Werror
+debug: OPTFLAGS += -g -Wall
 CFLAGS += $(OPTFLAGS)
 
 .PHONY: libiglmosek

include/igl/mosek/mosek_quadprog.cpp

@@ -91,15 +91,15 @@ IGL_INLINE bool igl::mosek_quadprog(
 
   // Create the MOSEK environment
   mosek_guarded(MSK_makeenv(&env,NULL,NULL,NULL,NULL));
-  /* Directs the log stream to the 'printstr' function. */
-  mosek_guarded(MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,printstr));
+  ///* Directs the log stream to the 'printstr' function. */
+  //mosek_guarded(MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,printstr));
   // initialize mosek environment
   mosek_guarded(MSK_initenv(env));
   // Create the optimization task
   mosek_guarded(MSK_maketask(env,m,n,&task));
   verbose("Creating task with %ld linear constraints and %ld variables...\n",m,n);
-  // Tell mosek how to print to std out
-  mosek_guarded(MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr));
+  //// Tell mosek how to print to std out
+  //mosek_guarded(MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr));
   // Give estimate of number of variables
   mosek_guarded(MSK_putmaxnumvar(task,n));
   if(m>0)
@@ -186,9 +186,9 @@ IGL_INLINE bool igl::mosek_quadprog(
   // run the optimizer
   mosek_guarded(MSK_optimizetrm(task,&trmcode));
 
-  // Print a summary containing information about the solution for debugging
-  // purposes
-  MSK_solutionsummary(task,MSK_STREAM_LOG);
+  //// Print a summary containing information about the solution for debugging
+  //// purposes
+  //MSK_solutionsummary(task,MSK_STREAM_LOG);
 
   // Get status of solution
   MSKsolstae solsta;
@@ -200,7 +200,7 @@ IGL_INLINE bool igl::mosek_quadprog(
     case MSK_SOL_STA_OPTIMAL:   
     case MSK_SOL_STA_NEAR_OPTIMAL:
       MSK_getsolutionslice(task,MSK_SOL_ITR,MSK_SOL_ITEM_XX,0,n,&x[0]);
-      printf("Optimal primal solution\n");
+      //printf("Optimal primal solution\n");
       //for(size_t j=0; j<n; ++j)
       //{
       //  printf("x[%ld]: %g\n",j,x[j]);
@@ -211,13 +211,13 @@ IGL_INLINE bool igl::mosek_quadprog(
     case MSK_SOL_STA_PRIM_INFEAS_CER:
     case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
     case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:  
-      printf("Primal or dual infeasibility certificate found.\n");
+      //printf("Primal or dual infeasibility certificate found.\n");
       break;
     case MSK_SOL_STA_UNKNOWN:
-      printf("The status of the solution could not be determined.\n");
+      //printf("The status of the solution could not be determined.\n");
       break;
     default:
-      printf("Other solution status.");
+      //printf("Other solution status.");
       break;
   }
 

include/igl/polar_dec.cpp

@@ -0,0 +1,46 @@
+#include "polar_dec.h"
+#include <Eigen/Eigenvalues>
+
+#include "polar_svd.h"
+#ifdef _WIN32
+#else
+#  include <fenv.h>
+#endif
+// You will need the development version of Eigen which is > 3.0.3
+// You can determine if you have computeDirect by issuing
+//   grep -r computeDirect path/to/eigen/*
+#define EIGEN_HAS_COMPUTE_DIRECT
+
+// From Olga's CGAL mentee's ARAP code
+template<typename Mat>
+IGL_INLINE void igl::polar_dec(const Mat& A, Mat& R, Mat& T)
+{
+#ifdef EIGEN_HAS_COMPUTE_DIRECT
+ typedef typename Mat::Scalar Scalar;
+ typedef Eigen::Matrix<typename Mat::Scalar,3,1> Vec;
+
+ const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());
+
+ Eigen::SelfAdjointEigenSolver<Mat> eig;
+ feclearexcept(FE_UNDERFLOW);
+ eig.computeDirect(A.transpose()*A);
+ if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
+   return polar_svd(A,R,T);
+
+ Vec S = eig.eigenvalues().cwiseSqrt();
+
+ T = eig.eigenvectors() * S.asDiagonal() * eig.eigenvectors().transpose();
+ R = A  * eig.eigenvectors() * S.asDiagonal().inverse()
+        * eig.eigenvectors().transpose();
+
+ if(std::abs(R.squaredNorm()-3.) > th)
+   return polar_svd(A,R,T);
+#else
+  return polar_svd(A,R,T);
+#endif
+}
+
+#ifndef IGL_HEADER_ONLY
+// Explicit template instanciation
+template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&);
+#endif

include/igl/polar_dec.h

@@ -0,0 +1,24 @@
+#ifndef IGL_POLAR_DEC
+#define IGL_POLAR_DEC
+#include "igl_inline.h"
+
+namespace igl
+{
+  // Computes the polar decomposition (R,T) of a matrix A
+  // Inputs:
+  //   A  3 by 3 matrix to be decomposed
+  // Outputs:
+  //   R  3 by 3 rotation matrix part of decomposition
+  //   T  3 by 3 stretch matrix part of decomposition
+  //
+  // Note: I'm not sure if this implementation is check against reflections in R
+  // Note: It is not
+  //
+  template<typename Mat>
+  IGL_INLINE void polar_dec(const Mat& A, Mat& R, Mat& T);
+}
+#ifdef IGL_HEADER_ONLY
+#  include "polar_dec.cpp"
+#endif
+#endif
+

include/igl/polar_svd.cpp

@@ -0,0 +1,53 @@
+#include "polar_svd.h"
+#include <Eigen/SVD>
+
+// From Olga's CGAL mentee's ARAP code
+template<typename Mat>
+IGL_INLINE void igl::polar_svd(const Mat& A, Mat& R, Mat& T)
+{
+  typedef Eigen::Matrix<typename Mat::Scalar,Mat::RowsAtCompileTime,1> Vec;
+  Eigen::JacobiSVD<Mat> svd;
+  svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
+  const Mat& u = svd.matrixU();
+  const Mat& v = svd.matrixV();
+  const Vec& w = svd.singularValues();
+  R = u*v.transpose();
+  T = v*w.asDiagonal()*v.adjoint();
+}
+
+IGL_INLINE void igl::polar_svd(const Eigen::Matrix3f& A, Eigen::Matrix3f& R, Eigen::Matrix3f& T)
+{
+  typedef Eigen::Matrix<Eigen::Matrix3f::Scalar,3,1> Vec;
+  Eigen::JacobiSVD<Eigen::Matrix3f> svd;
+  svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
+  const Eigen::Matrix3f& u = svd.matrixU();
+  const Eigen::Matrix3f& v = svd.matrixV();
+  const Vec& w = svd.singularValues();
+  R = u*v.transpose();
+  T = v*w.asDiagonal()*v.adjoint();
+}
+
+// Clang is giving an annoying warning inside Eigen
+#ifdef __clang__
+#  pragma clang diagnostic push
+#  pragma clang diagnostic ignored "-Wconstant-logical-operand"
+#endif
+IGL_INLINE void igl::polar_svd(const Eigen::Matrix2f& A, Eigen::Matrix2f& R, Eigen::Matrix2f& T)
+{
+  typedef Eigen::Matrix<Eigen::Matrix2f::Scalar,2,1> Vec;
+  Eigen::JacobiSVD<Eigen::Matrix2f> svd;
+  svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
+  const Eigen::Matrix2f& u = svd.matrixU();
+  const Eigen::Matrix2f& v = svd.matrixV();
+  const Vec& w = svd.singularValues();
+  R = u*v.transpose();
+  T = v*w.asDiagonal()*v.adjoint();
+}
+#ifdef __clang__
+#  pragma clang diagnostic pop
+#endif
+
+#ifndef IGL_HEADER_ONLY
+// Explicit template instanciation
+template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&);
+#endif

include/igl/polar_svd.h

@@ -0,0 +1,23 @@
+#ifndef IGL_POLAR_SVD
+#define IGL_POLAR_SVD
+#include "igl_inline.h"
+#include <Eigen/Core>
+
+namespace igl
+{
+  // Computes the polar decomposition (R,T) of a matrix A using SVD singular value decomposition
+  // Inputs:
+  //   A  3 by 3 matrix to be decomposed
+  // Outputs:
+  //   R  3 by 3 rotation matrix part of decomposition
+  //   T  3 by 3 stretch matrix part of decomposition
+  //
+  IGL_INLINE void polar_svd(const Eigen::Matrix3f& A, Eigen::Matrix3f& R, Eigen::Matrix3f& T);
+  IGL_INLINE void polar_svd(const Eigen::Matrix2f& A, Eigen::Matrix2f& R, Eigen::Matrix2f& T);
+  template<typename Mat>
+  IGL_INLINE void polar_svd(const Mat& A, Mat& R, Mat& T);
+}
+#ifdef IGL_HEADER_ONLY
+#  include "polar_svd.cpp"
+#endif
+#endif