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> 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> 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> 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> 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> 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> 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> 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 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
