1
2 #include <iostream>
3 #include <Eigen/Geometry>
4 #include <bench/BenchTimer.h>
5 using namespace Eigen;
6 using namespace std;
7
8
9
10 template<typename Q>
nlerp(const Q & a,const Q & b,typename Q::Scalar t)11 EIGEN_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
16 template<typename Q>
slerp_eigen(const Q & a,const Q & b,typename Q::Scalar t)17 EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
18 {
19 return a.slerp(t,b);
20 }
21
22 template<typename Q>
slerp_legacy(const Q & a,const Q & b,typename Q::Scalar t)23 EIGEN_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
44 template<typename Q>
slerp_legacy_nlerp(const Q & a,const Q & b,typename Q::Scalar t)45 EIGEN_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
75 template<typename T>
sin_over_x(T x)76 inline 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
84 template<typename Q>
slerp_rw(const Q & a,const Q & b,typename Q::Scalar t)85 EIGEN_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
108 template<typename Q>
slerp_gael(const Q & a,const Q & b,typename Q::Scalar t)109 EIGEN_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
main()144 int 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