123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247 |
- #include <iostream>
- #include <Eigen/Geometry>
- #include <bench/BenchTimer.h>
- using namespace Eigen;
- using namespace std;
- template<typename Q>
- 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<typename Q>
- EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
- {
- return a.slerp(t,b);
- }
- template<typename Q>
- 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>();
- Scalar d = a.dot(b);
- Scalar absD = internal::abs(d);
- if (absD>=one)
- return a;
-
- 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<typename Q>
- 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>();
- Scalar d = a.dot(b);
- Scalar absD = internal::abs(d);
-
- Scalar scale0;
- Scalar scale1;
-
- if (absD>=one)
- {
- scale0 = Scalar(1) - t;
- scale1 = t;
- }
- else
- {
-
- 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<typename T>
- inline T sin_over_x(T x)
- {
- if (T(1) + x*x == T(1))
- return T(1);
- else
- return std::sin(x)/x;
- }
- template<typename Q>
- 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 = Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
- else
- theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
-
-
- 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<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
- }
- template<typename Q>
- 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;
-
- if (d<0.0)
- theta = 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<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
- }
- int main()
- {
- typedef double RefScalar;
- typedef float TestScalar;
-
- typedef Quaternion<RefScalar> Qd;
- typedef Quaternion<TestScalar> Qf;
-
- unsigned int g_seed = (unsigned int) time(NULL);
- std::cout << g_seed << "\n";
- srand(g_seed);
-
- Matrix<RefScalar,Dynamic,1> maxerr(7);
- maxerr.setZero();
-
- Matrix<RefScalar,Dynamic,1> 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<rep; ++w)
- {
- Qf a, b;
- a.coeffs().setRandom();
- a.normalize();
- b.coeffs().setRandom();
- b.normalize();
-
- Qf c[6];
-
- Qd ar(a.cast<RefScalar>());
- Qd br(b.cast<RefScalar>());
- Qd cr;
-
-
-
- cout.precision(8);
- cout << std::scientific;
- for (int i=0; i<iters; ++i)
- {
- RefScalar t = 0.65;
- cr = slerp_rw(ar,br,t);
-
- Qf refc = cr.cast<TestScalar>();
- 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<RefScalar>().coeffs()).norm();
- for (int k=0; k<6; ++k)
- {
- err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
- }
- maxerr = maxerr.cwise().max(err);
- avgerr += err;
- b = cr.cast<TestScalar>();
- br = cr;
- }
- }
- avgerr /= RefScalar(rep*iters);
- cout << "\n\nAccuracy:\n"
- << " max: " << maxerr.transpose() << "\n";
- cout << " avg: " << avgerr.transpose() << "\n";
-
-
- Quaternionf a,b;
- a.coeffs().setRandom();
- a.normalize();
- b.coeffs().setRandom();
- b.normalize();
-
- 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);
- }
|