quat_slerp.cpp File Reference

#include <iostream>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

Macros
#define	BENCH(FUNC)

Functions
template<typename Q >
EIGEN_DONT_INLINE Q	nlerp (const Q &a, const Q &b, typename Q::Scalar t)
template<typename Q >
EIGEN_DONT_INLINE Q	slerp_eigen (const Q &a, const Q &b, typename Q::Scalar t)
template<typename Q >
EIGEN_DONT_INLINE Q	slerp_legacy (const Q &a, const Q &b, typename Q::Scalar t)
template<typename Q >
EIGEN_DONT_INLINE Q	slerp_legacy_nlerp (const Q &a, const Q &b, typename Q::Scalar t)
template<typename T >
T	sin_over_x (T x)
template<typename Q >
EIGEN_DONT_INLINE Q	slerp_rw (const Q &a, const Q &b, typename Q::Scalar t)
template<typename Q >
EIGEN_DONT_INLINE Q	slerp_gael (const Q &a, const Q &b, typename Q::Scalar t)
int	main ()

Macro Definition Documentation

BENCH

#define BENCH

(

FUNC

)

Value:

  {                                                           \
    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"; \
  }

Function Documentation

main()

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);
}

References a, b, BENCH, calibrate::c, Eigen::g_seed, i, int(), k, nlerp(), s, Eigen::PlainObjectBase< Derived >::setZero(), slerp_eigen(), slerp_gael(), slerp_legacy(), slerp_legacy_nlerp(), slerp_rw(), plotPSD::t, and w.

nlerp()

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());
}

References a, b, Q, and plotPSD::t.

Referenced by main().

sin_over_x()

template<typename T >

T sin_over_x ( T  x )

inline

                         {
  if (T(1) + x * x == T(1))
    return T(1);
  else
    return std::sin(x) / x;
}

References sin(), and plotDoE::x.

Referenced by slerp_rw().

slerp_eigen()

template<typename Q >

EIGEN_DONT_INLINE Q slerp_eigen	(	const Q &	a,
		const Q &	b,
		typename Q::Scalar	t
	)

                                                                            {
  return a.slerp(t, b);
}

References a, b, and plotPSD::t.

Referenced by main().

slerp_gael()

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 = internal::sin((Scalar(1) - t) * theta) / sinTheta;
    scale1 = internal::sin((t * theta)) / sinTheta;
    if (d < 0) scale1 = -scale1;
  }
 
  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
}

References a, Eigen::bfloat16_impl::asin(), b, sin(), plotPSD::t, and BiharmonicTestFunctions2::theta.

Referenced by main().

slerp_legacy()

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 = internal::abs(d);
  if (absD >= one) return a;
 
  // theta is the angle between the 2 quaternions
  Scalar theta = std::acos(absD);
  Scalar sinTheta = internal::sin(theta);
 
  Scalar scale0 = internal::sin((Scalar(1) - t) * theta) / sinTheta;
  Scalar scale1 = internal::sin((t * theta)) / sinTheta;
  if (d < 0) scale1 = -scale1;
 
  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
}

References a, abs(), acos(), b, Q, sin(), plotPSD::t, and BiharmonicTestFunctions2::theta.

Referenced by main().

slerp_legacy_nlerp()

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 = internal::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 = internal::sin(theta);
 
    scale0 = internal::sin((Scalar(1) - t) * theta) / sinTheta;
    scale1 = internal::sin((t * theta)) / sinTheta;
    if (d < 0) scale1 = -scale1;
  }
 
  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
}

References a, abs(), acos(), b, Q, sin(), plotPSD::t, and BiharmonicTestFunctions2::theta.

Referenced by main().

slerp_rw()

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());
}

References a, Eigen::bfloat16_impl::asin(), b, sin_over_x(), plotPSD::t, and BiharmonicTestFunctions2::theta.

Referenced by main().