1
2#include <iostream>
3#include <Eigen/Geometry>
4#include <bench/BenchTimer.h>
5using namespace Eigen;
6using namespace std;
7
8
9
10template<typename Q>
11EIGEN_DONT_INLINE Q nlerp(const Q& a, const Q& b, typename Q::Scalar t)
12{
13  return Q((a.coeffs() * (1.0-t) + b.coeffs() * t).normalized());
14}
15
16template<typename Q>
17EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
18{
19  return a.slerp(t,b);
20}
21
22template<typename Q>
23EIGEN_DONT_INLINE Q slerp_legacy(const Q& a, const Q& b, typename Q::Scalar t)
24{
25  typedef typename Q::Scalar Scalar;
26  static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
27  Scalar d = a.dot(b);
28  Scalar absD = internal::abs(d);
29  if (absD>=one)
30    return a;
31
32  // theta is the angle between the 2 quaternions
33  Scalar theta = std::acos(absD);
34  Scalar sinTheta = internal::sin(theta);
35
36  Scalar scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
37  Scalar scale1 = internal::sin( ( t * theta) ) / sinTheta;
38  if (d<0)
39    scale1 = -scale1;
40
41  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
42}
43
44template<typename Q>
45EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q& a, const Q& b, typename Q::Scalar t)
46{
47  typedef typename Q::Scalar Scalar;
48  static const Scalar one = Scalar(1) - epsilon<Scalar>();
49  Scalar d = a.dot(b);
50  Scalar absD = internal::abs(d);
51
52  Scalar scale0;
53  Scalar scale1;
54
55  if (absD>=one)
56  {
57    scale0 = Scalar(1) - t;
58    scale1 = t;
59  }
60  else
61  {
62    // theta is the angle between the 2 quaternions
63    Scalar theta = std::acos(absD);
64    Scalar sinTheta = internal::sin(theta);
65
66    scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
67    scale1 = internal::sin( ( t * theta) ) / sinTheta;
68    if (d<0)
69      scale1 = -scale1;
70  }
71
72  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
73}
74
75template<typename T>
76inline T sin_over_x(T x)
77{
78  if (T(1) + x*x == T(1))
79    return T(1);
80  else
81    return std::sin(x)/x;
82}
83
84template<typename Q>
85EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
86{
87  typedef typename Q::Scalar Scalar;
88
89  Scalar d = a.dot(b);
90  Scalar theta;
91  if (d<0.0)
92    theta = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
93  else
94    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
95
96  // theta is the angle between the 2 quaternions
97//   Scalar theta = std::acos(absD);
98  Scalar sinOverTheta = sin_over_x(theta);
99
100  Scalar scale0 = (Scalar(1)-t)*sin_over_x( ( Scalar(1) - t ) * theta) / sinOverTheta;
101  Scalar scale1 = t * sin_over_x( ( t * theta) ) / sinOverTheta;
102  if (d<0)
103    scale1 = -scale1;
104
105  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
106}
107
108template<typename Q>
109EIGEN_DONT_INLINE Q slerp_gael(const Q& a, const Q& b, typename Q::Scalar t)
110{
111  typedef typename Q::Scalar Scalar;
112
113  Scalar d = a.dot(b);
114  Scalar theta;
115//   theta = Scalar(2) * atan2((a.coeffs()-b.coeffs()).norm(),(a.coeffs()+b.coeffs()).norm());
116//   if (d<0.0)
117//     theta = M_PI-theta;
118
119  if (d<0.0)
120    theta = /*M_PI -*/ Scalar(2)*std::asin( (-a.coeffs()-b.coeffs()).norm()/2 );
121  else
122    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
123
124
125  Scalar scale0;
126  Scalar scale1;
127  if(theta*theta-Scalar(6)==-Scalar(6))
128  {
129    scale0 = Scalar(1) - t;
130    scale1 = t;
131  }
132  else
133  {
134    Scalar sinTheta = std::sin(theta);
135    scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
136    scale1 = internal::sin( ( t * theta) ) / sinTheta;
137    if (d<0)
138      scale1 = -scale1;
139  }
140
141  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
142}
143
144int main()
145{
146  typedef double RefScalar;
147  typedef float TestScalar;
148
149  typedef Quaternion<RefScalar>  Qd;
150  typedef Quaternion<TestScalar> Qf;
151
152  unsigned int g_seed = (unsigned int) time(NULL);
153  std::cout << g_seed << "\n";
154//   g_seed = 1259932496;
155  srand(g_seed);
156
157  Matrix<RefScalar,Dynamic,1> maxerr(7);
158  maxerr.setZero();
159
160  Matrix<RefScalar,Dynamic,1> avgerr(7);
161  avgerr.setZero();
162
163  cout << "double=>float=>double       nlerp        eigen        legacy(snap)         legacy(nlerp)        rightway         gael's criteria\n";
164
165  int rep = 100;
166  int iters = 40;
167  for (int w=0; w<rep; ++w)
168  {
169    Qf a, b;
170    a.coeffs().setRandom();
171    a.normalize();
172    b.coeffs().setRandom();
173    b.normalize();
174
175    Qf c[6];
176
177    Qd ar(a.cast<RefScalar>());
178    Qd br(b.cast<RefScalar>());
179    Qd cr;
180
181
182
183    cout.precision(8);
184    cout << std::scientific;
185    for (int i=0; i<iters; ++i)
186    {
187      RefScalar t = 0.65;
188      cr = slerp_rw(ar,br,t);
189
190      Qf refc = cr.cast<TestScalar>();
191      c[0] = nlerp(a,b,t);
192      c[1] = slerp_eigen(a,b,t);
193      c[2] = slerp_legacy(a,b,t);
194      c[3] = slerp_legacy_nlerp(a,b,t);
195      c[4] = slerp_rw(a,b,t);
196      c[5] = slerp_gael(a,b,t);
197
198      VectorXd err(7);
199      err[0] = (cr.coeffs()-refc.cast<RefScalar>().coeffs()).norm();
200//       std::cout << err[0] << "    ";
201      for (int k=0; k<6; ++k)
202      {
203        err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
204//         std::cout << err[k+1] << "    ";
205      }
206      maxerr = maxerr.cwise().max(err);
207      avgerr += err;
208//       std::cout << "\n";
209      b = cr.cast<TestScalar>();
210      br = cr;
211    }
212//     std::cout << "\n";
213  }
214  avgerr /= RefScalar(rep*iters);
215  cout << "\n\nAccuracy:\n"
216       << "  max: " << maxerr.transpose() << "\n";
217  cout << "  avg: " << avgerr.transpose() << "\n";
218
219  // perf bench
220  Quaternionf a,b;
221  a.coeffs().setRandom();
222  a.normalize();
223  b.coeffs().setRandom();
224  b.normalize();
225  //b = a;
226  float s = 0.65;
227
228  #define BENCH(FUNC) {\
229    BenchTimer t; \
230    for(int k=0; k<2; ++k) {\
231      t.start(); \
232      for(int i=0; i<1000000; ++i) \
233        FUNC(a,b,s); \
234      t.stop(); \
235    } \
236    cout << "  " << #FUNC << " => \t " << t.value() << "s\n"; \
237  }
238
239  cout << "\nSpeed:\n" << std::fixed;
240  BENCH(nlerp);
241  BENCH(slerp_eigen);
242  BENCH(slerp_legacy);
243  BENCH(slerp_legacy_nlerp);
244  BENCH(slerp_rw);
245  BENCH(slerp_gael);
246}
247
248