blob: bffb3bf11bc842616c976477ab18f38dd4e60fe6 [file] [log] [blame]
 #include #include #include using namespace Eigen; using namespace std; template 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 EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t) { return a.slerp(t,b); } template 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 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()); } template 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 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()); } template inline T sin_over_x(T x) { if (T(1) + x*x == T(1)) return T(1); else return std::sin(x)/x; } template 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(scale0 * a.coeffs() + scale1 * b.coeffs()); } template 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(scale0 * a.coeffs() + scale1 * b.coeffs()); } int main() { typedef double RefScalar; typedef float TestScalar; typedef Quaternion Qd; typedef Quaternion Qf; unsigned int g_seed = (unsigned int) time(NULL); std::cout << g_seed << "\n"; // g_seed = 1259932496; srand(g_seed); Matrix maxerr(7); maxerr.setZero(); Matrix 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()); Qd br(b.cast()); Qd cr; cout.precision(8); cout << std::scientific; for (int i=0; i(); 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().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(); 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); }