add a slerp benchmark (for accuracy and speed))

1 files changed, 245 insertions, 0 deletions

diff --git a/bench/quat_slerp.cpp b/bench/quat_slerp.cpp
new file mode 100644
index 000000000..029443704
--- /dev/null
+++ b/bench/quat_slerp.cpp
@@ -0,0 +1,245 @@
+
+#include <Eigen/Geometry>
+#include <bench/BenchTimer.h>
+using namespace Eigen;
+using namespace std;
+
+
+
+template<typename Q>
+EIGEN_DONT_INLINE Q nlerp(const Q& a, const Q& b, typename Q::Scalar t)
+{
+  return Q((a.coeffs() * (1.0-t) + b.coeffs() * t).normalized());
+}
+
+template<typename Q>
+EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
+{
+  return a.slerp(t,b);
+}
+
+template<typename Q>
+EIGEN_DONT_INLINE Q slerp_legacy(const Q& a, const Q& b, typename Q::Scalar t)
+{
+  typedef typename Q::Scalar Scalar;
+  static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
+  Scalar d = a.dot(b);
+  Scalar absD = ei_abs(d);
+  if (absD>=one)
+    return a;
+
+  // theta is the angle between the 2 quaternions
+  Scalar theta = std::acos(absD);
+  Scalar sinTheta = ei_sin(theta);
+
+  Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
+  Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta;
+  if (d<0)
+    scale1 = -scale1;
+
+  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
+}
+
+template<typename Q>
+EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q& a, const Q& b, typename Q::Scalar t)
+{
+  typedef typename Q::Scalar Scalar;
+  static const Scalar one = Scalar(1) - epsilon<Scalar>();
+  Scalar d = a.dot(b);
+  Scalar absD = ei_abs(d);
+  
+  Scalar scale0;
+  Scalar scale1;
+  
+  if (absD>=one)
+  {
+    scale0 = Scalar(1) - t;
+    scale1 = t;
+  }
+  else
+  {
+    // theta is the angle between the 2 quaternions
+    Scalar theta = std::acos(absD);
+    Scalar sinTheta = ei_sin(theta);
+
+    scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
+    scale1 = ei_sin( ( t * theta) ) / sinTheta;
+    if (d<0)
+      scale1 = -scale1;
+  }
+
+  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
+}
+
+template<typename T>
+inline T sin_over_x(T x)
+{
+  if (T(1) + x*x == T(1))
+    return T(1);
+  else
+    return std::sin(x)/x;
+}
+
+template<typename Q>
+EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
+{
+  typedef typename Q::Scalar Scalar;
+  
+  Scalar d = a.dot(b);
+  Scalar theta;
+  if (d<0.0)
+    theta = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
+  else
+    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
+  
+  // theta is the angle between the 2 quaternions
+//   Scalar theta = std::acos(absD);
+  Scalar sinOverTheta = sin_over_x(theta);
+
+  Scalar scale0 = (Scalar(1)-t)*sin_over_x( ( Scalar(1) - t ) * theta) / sinOverTheta;
+  Scalar scale1 = t * sin_over_x( ( t * theta) ) / sinOverTheta;
+  if (d<0)
+    scale1 = -scale1;
+
+  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
+}
+
+template<typename Q>
+EIGEN_DONT_INLINE Q slerp_gael(const Q& a, const Q& b, typename Q::Scalar t)
+{
+  typedef typename Q::Scalar Scalar;
+  
+  Scalar d = a.dot(b);
+  Scalar theta;
+//   theta = Scalar(2) * atan2((a.coeffs()-b.coeffs()).norm(),(a.coeffs()+b.coeffs()).norm());
+//   if (d<0.0)
+//     theta = M_PI-theta;
+  
+  if (d<0.0)
+    theta = /*M_PI -*/ Scalar(2)*std::asin( (-a.coeffs()-b.coeffs()).norm()/2 );
+  else
+    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
+  
+  
+  Scalar scale0;
+  Scalar scale1;
+  if(theta*theta-Scalar(6)==-Scalar(6))
+  {
+    scale0 = Scalar(1) - t;
+    scale1 = t;
+  }
+  else
+  {
+    Scalar sinTheta = std::sin(theta);
+    scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
+    scale1 = ei_sin( ( t * theta) ) / sinTheta;
+    if (d<0)
+      scale1 = -scale1;
+  }
+
+  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
+}
+
+int main()
+{
+  typedef double RefScalar;
+  typedef float TestScalar;
+  
+  typedef Quaternion<RefScalar>  Qd;
+  typedef Quaternion<TestScalar> Qf;
+  
+  unsigned int g_seed = (unsigned int) time(NULL);
+  std::cout << g_seed << "\n";
+//   g_seed = 1259932496;
+  srand(g_seed);
+  
+  Matrix<RefScalar,Dynamic,1> maxerr(7);
+  maxerr.setZero();
+  
+  Matrix<RefScalar,Dynamic,1> avgerr(7);
+  avgerr.setZero();
+  
+  cout << "double=>float=>double       nlerp        eigen        legacy(snap)         legacy(nlerp)        rightway         gael's criteria\n";
+  
+  int rep = 100;
+  int iters = 40;
+  for (int w=0; w<rep; ++w)
+  {
+    Qf a, b;
+    a.coeffs().setRandom();
+    a.normalize();
+    b.coeffs().setRandom();
+    b.normalize();
+    
+    Qf c[6];
+    
+    Qd ar(a.cast<RefScalar>());
+    Qd br(b.cast<RefScalar>());
+    Qd cr;
+    
+    
+    
+    cout.precision(8);
+    cout << std::scientific;
+    for (int i=0; i<iters; ++i)
+    {
+      RefScalar t = 0.65;
+      cr = slerp_rw(ar,br,t);
+      
+      Qf refc = cr.cast<TestScalar>();
+      c[0] = nlerp(a,b,t);
+      c[1] = slerp_eigen(a,b,t);
+      c[2] = slerp_legacy(a,b,t);
+      c[3] = slerp_legacy_nlerp(a,b,t);
+      c[4] = slerp_rw(a,b,t);
+      c[5] = slerp_gael(a,b,t);
+      
+      VectorXd err(7);
+      err[0] = (cr.coeffs()-refc.cast<RefScalar>().coeffs()).norm();
+//       std::cout << err[0] << "    ";
+      for (int k=0; k<6; ++k)
+      {
+        err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
+//         std::cout << err[k+1] << "    ";
+      }
+      maxerr = maxerr.cwise().max(err);
+      avgerr += err;
+//       std::cout << "\n";
+      b = cr.cast<TestScalar>();
+      br = cr;
+    }
+//     std::cout << "\n";
+  }
+  avgerr /= RefScalar(rep*iters);
+  cout << "\n\nAccuracy:\n"
+       << "  max: " << maxerr.transpose() << "\n";
+  cout << "  avg: " << avgerr.transpose() << "\n";
+  
+  // perf bench
+  Quaternionf a,b;
+  a.coeffs().setRandom();
+  a.normalize();
+  b.coeffs().setRandom();
+  b.normalize();
+  //b = a;
+  float s = 0.65;
+    
+  #define BENCH(FUNC) {\
+    BenchTimer t; \
+    for(int k=0; k<2; ++k) {\
+      t.start(); \
+      for(int i=0; i<1000000; ++i) \
+        FUNC(a,b,s); \
+      t.stop(); \
+    } \
+    cout << "  " << #FUNC << " => \t " << t.value() << "s\n"; \
+  }
+  
+  cout << "\nSpeed:\n" << std::fixed;
+  BENCH(nlerp);
+  BENCH(slerp_eigen);
+  BENCH(slerp_legacy);
+  BENCH(slerp_legacy_nlerp);
+  BENCH(slerp_rw);
+  BENCH(slerp_gael);
+}
\ No newline at end of file