xref: /aosp_15_r20/external/eigen/bench/geometry.cpp (revision bf2c37156dfe67e5dfebd6d394bad8b2ab5804d4)
1*bf2c3715SXin Li 
2*bf2c3715SXin Li #include <iostream>
3*bf2c3715SXin Li #include <Eigen/Geometry>
4*bf2c3715SXin Li #include <bench/BenchTimer.h>
5*bf2c3715SXin Li 
6*bf2c3715SXin Li using namespace std;
7*bf2c3715SXin Li using namespace Eigen;
8*bf2c3715SXin Li 
9*bf2c3715SXin Li #ifndef SCALAR
10*bf2c3715SXin Li #define SCALAR float
11*bf2c3715SXin Li #endif
12*bf2c3715SXin Li 
13*bf2c3715SXin Li #ifndef SIZE
14*bf2c3715SXin Li #define SIZE 8
15*bf2c3715SXin Li #endif
16*bf2c3715SXin Li 
17*bf2c3715SXin Li typedef SCALAR Scalar;
18*bf2c3715SXin Li typedef NumTraits<Scalar>::Real RealScalar;
19*bf2c3715SXin Li typedef Matrix<RealScalar,Dynamic,Dynamic> A;
20*bf2c3715SXin Li typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
21*bf2c3715SXin Li typedef Matrix<Scalar,Dynamic,Dynamic> C;
22*bf2c3715SXin Li typedef Matrix<RealScalar,Dynamic,Dynamic> M;
23*bf2c3715SXin Li 
24*bf2c3715SXin Li template<typename Transformation, typename Data>
transform(const Transformation & t,Data & data)25*bf2c3715SXin Li EIGEN_DONT_INLINE void transform(const Transformation& t, Data& data)
26*bf2c3715SXin Li {
27*bf2c3715SXin Li   EIGEN_ASM_COMMENT("begin");
28*bf2c3715SXin Li   data = t * data;
29*bf2c3715SXin Li   EIGEN_ASM_COMMENT("end");
30*bf2c3715SXin Li }
31*bf2c3715SXin Li 
32*bf2c3715SXin Li template<typename Scalar, typename Data>
transform(const Quaternion<Scalar> & t,Data & data)33*bf2c3715SXin Li EIGEN_DONT_INLINE void transform(const Quaternion<Scalar>& t, Data& data)
34*bf2c3715SXin Li {
35*bf2c3715SXin Li   EIGEN_ASM_COMMENT("begin quat");
36*bf2c3715SXin Li   for(int i=0;i<data.cols();++i)
37*bf2c3715SXin Li     data.col(i) = t * data.col(i);
38*bf2c3715SXin Li   EIGEN_ASM_COMMENT("end quat");
39*bf2c3715SXin Li }
40*bf2c3715SXin Li 
41*bf2c3715SXin Li template<typename T> struct ToRotationMatrixWrapper
42*bf2c3715SXin Li {
43*bf2c3715SXin Li   enum {Dim = T::Dim};
44*bf2c3715SXin Li   typedef typename T::Scalar Scalar;
ToRotationMatrixWrapperToRotationMatrixWrapper45*bf2c3715SXin Li   ToRotationMatrixWrapper(const T& o) : object(o) {}
46*bf2c3715SXin Li   T object;
47*bf2c3715SXin Li };
48*bf2c3715SXin Li 
49*bf2c3715SXin Li template<typename QType, typename Data>
transform(const ToRotationMatrixWrapper<QType> & t,Data & data)50*bf2c3715SXin Li EIGEN_DONT_INLINE void transform(const ToRotationMatrixWrapper<QType>& t, Data& data)
51*bf2c3715SXin Li {
52*bf2c3715SXin Li   EIGEN_ASM_COMMENT("begin quat via mat");
53*bf2c3715SXin Li   data = t.object.toRotationMatrix() * data;
54*bf2c3715SXin Li   EIGEN_ASM_COMMENT("end quat via mat");
55*bf2c3715SXin Li }
56*bf2c3715SXin Li 
57*bf2c3715SXin Li template<typename Scalar, int Dim, typename Data>
transform(const Transform<Scalar,Dim,Projective> & t,Data & data)58*bf2c3715SXin Li EIGEN_DONT_INLINE void transform(const Transform<Scalar,Dim,Projective>& t, Data& data)
59*bf2c3715SXin Li {
60*bf2c3715SXin Li   data = (t * data.colwise().homogeneous()).template block<Dim,Data::ColsAtCompileTime>(0,0);
61*bf2c3715SXin Li }
62*bf2c3715SXin Li 
63*bf2c3715SXin Li template<typename T> struct get_dim { enum { Dim = T::Dim }; };
64*bf2c3715SXin Li template<typename S, int R, int C, int O, int MR, int MC>
65*bf2c3715SXin Li struct get_dim<Matrix<S,R,C,O,MR,MC> > { enum { Dim = R }; };
66*bf2c3715SXin Li 
67*bf2c3715SXin Li template<typename Transformation, int N>
68*bf2c3715SXin Li struct bench_impl
69*bf2c3715SXin Li {
runbench_impl70*bf2c3715SXin Li   static EIGEN_DONT_INLINE void run(const Transformation& t)
71*bf2c3715SXin Li   {
72*bf2c3715SXin Li     Matrix<typename Transformation::Scalar,get_dim<Transformation>::Dim,N> data;
73*bf2c3715SXin Li     data.setRandom();
74*bf2c3715SXin Li     bench_impl<Transformation,N-1>::run(t);
75*bf2c3715SXin Li     BenchTimer timer;
76*bf2c3715SXin Li     BENCH(timer,10,100000,transform(t,data));
77*bf2c3715SXin Li     cout.width(9);
78*bf2c3715SXin Li     cout << timer.best() << " ";
79*bf2c3715SXin Li   }
80*bf2c3715SXin Li };
81*bf2c3715SXin Li 
82*bf2c3715SXin Li 
83*bf2c3715SXin Li template<typename Transformation>
84*bf2c3715SXin Li struct bench_impl<Transformation,0>
85*bf2c3715SXin Li {
runbench_impl86*bf2c3715SXin Li   static EIGEN_DONT_INLINE void run(const Transformation&) {}
87*bf2c3715SXin Li };
88*bf2c3715SXin Li 
89*bf2c3715SXin Li template<typename Transformation>
bench(const std::string & msg,const Transformation & t)90*bf2c3715SXin Li EIGEN_DONT_INLINE void bench(const std::string& msg, const Transformation& t)
91*bf2c3715SXin Li {
92*bf2c3715SXin Li   cout << msg << " ";
93*bf2c3715SXin Li   bench_impl<Transformation,SIZE>::run(t);
94*bf2c3715SXin Li   std::cout << "\n";
95*bf2c3715SXin Li }
96*bf2c3715SXin Li 
main(int argc,char ** argv)97*bf2c3715SXin Li int main(int argc, char ** argv)
98*bf2c3715SXin Li {
99*bf2c3715SXin Li   Matrix<Scalar,3,4> mat34; mat34.setRandom();
100*bf2c3715SXin Li   Transform<Scalar,3,Isometry> iso3(mat34);
101*bf2c3715SXin Li   Transform<Scalar,3,Affine> aff3(mat34);
102*bf2c3715SXin Li   Transform<Scalar,3,AffineCompact> caff3(mat34);
103*bf2c3715SXin Li   Transform<Scalar,3,Projective> proj3(mat34);
104*bf2c3715SXin Li   Quaternion<Scalar> quat;quat.setIdentity();
105*bf2c3715SXin Li   ToRotationMatrixWrapper<Quaternion<Scalar> > quatmat(quat);
106*bf2c3715SXin Li   Matrix<Scalar,3,3> mat33; mat33.setRandom();
107*bf2c3715SXin Li 
108*bf2c3715SXin Li   cout.precision(4);
109*bf2c3715SXin Li   std::cout
110*bf2c3715SXin Li      << "N          ";
111*bf2c3715SXin Li   for(int i=0;i<SIZE;++i)
112*bf2c3715SXin Li   {
113*bf2c3715SXin Li     cout.width(9);
114*bf2c3715SXin Li     cout << i+1 << " ";
115*bf2c3715SXin Li   }
116*bf2c3715SXin Li   cout << "\n";
117*bf2c3715SXin Li 
118*bf2c3715SXin Li   bench("matrix 3x3", mat33);
119*bf2c3715SXin Li   bench("quaternion", quat);
120*bf2c3715SXin Li   bench("quat-mat  ", quatmat);
121*bf2c3715SXin Li   bench("isometry3 ", iso3);
122*bf2c3715SXin Li   bench("affine3   ", aff3);
123*bf2c3715SXin Li   bench("c affine3 ", caff3);
124*bf2c3715SXin Li   bench("proj3     ", proj3);
125*bf2c3715SXin Li }
126*bf2c3715SXin Li 
127