mirror of
https://gitlab.com/libeigen/eigen.git
synced 2026-04-10 11:34:33 +08:00
Clang-format tests, examples, libraries, benchmarks, etc.
This commit is contained in:
Antonio Sánchez
committed by
Rasmus Munk Larsen
Rasmus Munk Larsen
parent
3252ecc7a4
commit
46e9cdb7fe
@@ -5,237 +5,211 @@
|
||||
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 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_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)
|
||||
{
|
||||
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;
|
||||
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;
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
|
||||
if (absD >= one) {
|
||||
scale0 = Scalar(1) - t;
|
||||
scale1 = t;
|
||||
}
|
||||
else
|
||||
{
|
||||
} 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;
|
||||
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))
|
||||
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;
|
||||
return std::sin(x) / x;
|
||||
}
|
||||
|
||||
template<typename Q>
|
||||
EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
|
||||
{
|
||||
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 = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
|
||||
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 = Scalar(2) * std::asin((a.coeffs() - b.coeffs()).norm() / 2);
|
||||
|
||||
// theta is the angle between the 2 quaternions
|
||||
// Scalar theta = std::acos(absD);
|
||||
// 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;
|
||||
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)
|
||||
{
|
||||
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;
|
||||
// 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 );
|
||||
// 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 );
|
||||
|
||||
|
||||
theta = Scalar(2) * std::asin((a.coeffs() - b.coeffs()).norm() / 2);
|
||||
|
||||
Scalar scale0;
|
||||
Scalar scale1;
|
||||
if(theta*theta-Scalar(6)==-Scalar(6))
|
||||
{
|
||||
if (theta * theta - Scalar(6) == -Scalar(6)) {
|
||||
scale0 = Scalar(1) - t;
|
||||
scale1 = t;
|
||||
}
|
||||
else
|
||||
{
|
||||
} 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;
|
||||
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()
|
||||
{
|
||||
int main() {
|
||||
typedef double RefScalar;
|
||||
typedef float TestScalar;
|
||||
|
||||
typedef Quaternion<RefScalar> Qd;
|
||||
|
||||
typedef Quaternion<RefScalar> Qd;
|
||||
typedef Quaternion<TestScalar> Qf;
|
||||
|
||||
unsigned int g_seed = (unsigned int) time(NULL);
|
||||
|
||||
unsigned int g_seed = (unsigned int)time(NULL);
|
||||
std::cout << g_seed << "\n";
|
||||
// g_seed = 1259932496;
|
||||
// g_seed = 1259932496;
|
||||
srand(g_seed);
|
||||
|
||||
Matrix<RefScalar,Dynamic,1> maxerr(7);
|
||||
|
||||
Matrix<RefScalar, Dynamic, 1> maxerr(7);
|
||||
maxerr.setZero();
|
||||
|
||||
Matrix<RefScalar,Dynamic,1> avgerr(7);
|
||||
|
||||
Matrix<RefScalar, Dynamic, 1> avgerr(7);
|
||||
avgerr.setZero();
|
||||
|
||||
cout << "double=>float=>double nlerp eigen legacy(snap) legacy(nlerp) rightway gael's criteria\n";
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
for (int i = 0; i < iters; ++i) {
|
||||
RefScalar t = 0.65;
|
||||
cr = slerp_rw(ar,br,t);
|
||||
|
||||
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);
|
||||
|
||||
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();
|
||||
// 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] << " ";
|
||||
err[0] = (cr.coeffs() - refc.cast<RefScalar>().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";
|
||||
// std::cout << "\n";
|
||||
b = cr.cast<TestScalar>();
|
||||
br = cr;
|
||||
}
|
||||
// std::cout << "\n";
|
||||
// std::cout << "\n";
|
||||
}
|
||||
avgerr /= RefScalar(rep*iters);
|
||||
avgerr /= RefScalar(rep * iters);
|
||||
cout << "\n\nAccuracy:\n"
|
||||
<< " max: " << maxerr.transpose() << "\n";
|
||||
cout << " avg: " << avgerr.transpose() << "\n";
|
||||
|
||||
|
||||
// perf bench
|
||||
Quaternionf a,b;
|
||||
Quaternionf a, b;
|
||||
a.coeffs().setRandom();
|
||||
a.normalize();
|
||||
b.coeffs().setRandom();
|
||||
b.normalize();
|
||||
//b = a;
|
||||
// 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(); \
|
||||
} \
|
||||
|
||||
#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);
|
||||
@@ -244,4 +218,3 @@ int main()
|
||||
BENCH(slerp_rw);
|
||||
BENCH(slerp_gael);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user