From 36969cc2a5c2d0d3a52510b68c203b920eb4d3de Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Fri, 4 Dec 2009 15:02:38 +0100 Subject: add a slerp benchmark (for accuracy and speed)) --- bench/quat_slerp.cpp | 245 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 245 insertions(+) create mode 100644 bench/quat_slerp.cpp (limited to 'bench/quat_slerp.cpp') diff --git a/bench/quat_slerp.cpp b/bench/quat_slerp.cpp new file mode 100644 index 000000000..029443704 --- /dev/null +++ b/bench/quat_slerp.cpp @@ -0,0 +1,245 @@ + +#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 = ei_abs(d); + if (absD>=one) + return a; + + // theta is the angle between the 2 quaternions + Scalar theta = std::acos(absD); + Scalar sinTheta = ei_sin(theta); + + Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; + Scalar scale1 = ei_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 = ei_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 = ei_sin(theta); + + scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; + scale1 = ei_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 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta; + scale1 = ei_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); +} \ No newline at end of file -- cgit v1.2.3