ground/gcs/src/libs/eigen/test/prec_inverse_4x4.cpp

   1 // This file is part of Eigen, a lightweight C++ template library
   2 // for linear algebra.
   3 //
   4 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
   5 //
   6 // This Source Code Form is subject to the terms of the Mozilla
   7 // Public License v. 2.0. If a copy of the MPL was not distributed
   8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
   9
  10 #include "main.h"
  11 #include <Eigen/LU>
  12 #include <algorithm>
  13
  14 template<typename MatrixType> void inverse_permutation_4x4()
  15 {
  16   typedef typename MatrixType::Scalar Scalar;
  17   Vector4i indices(0,1,2,3);
  18   for(int i = 0; i < 24; ++i)
  19   {
  20     MatrixType m = PermutationMatrix<4>(indices);
  21     MatrixType inv = m.inverse();
  22     double error = double( (m*inv-MatrixType::Identity()).norm() / NumTraits<Scalar>::epsilon() );
  23     EIGEN_DEBUG_VAR(error)
  24     VERIFY(error == 0.0);
  25     std::next_permutation(indices.data(),indices.data()+4);
  26   }
  27 }
  28
  29 template<typename MatrixType> void inverse_general_4x4(int repeat)
  30 {
  31   using std::abs;
  32   typedef typename MatrixType::Scalar Scalar;
  33   typedef typename MatrixType::RealScalar RealScalar;
  34   double error_sum = 0., error_max = 0.;
  35   for(int i = 0; i < repeat; ++i)
  36   {
  37     MatrixType m;
  38     RealScalar absdet;
  39     do {
  40       m = MatrixType::Random();
  41       absdet = abs(m.determinant());
  42     } while(absdet < NumTraits<Scalar>::epsilon());
  43     MatrixType inv = m.inverse();
  44     double error = double( (m*inv-MatrixType::Identity()).norm() * absdet / NumTraits<Scalar>::epsilon() );
  45     error_sum += error;
  46     error_max = (std::max)(error_max, error);
  47   }
  48   std::cerr << "inverse_general_4x4, Scalar = " << type_name<Scalar>() << std::endl;
  49   double error_avg = error_sum / repeat;
  50   EIGEN_DEBUG_VAR(error_avg);
  51   EIGEN_DEBUG_VAR(error_max);
  52    // FIXME that 1.25 used to be a 1.0 until the NumTraits changes on 28 April 2010, what's going wrong??
  53    // FIXME that 1.25 used to be 1.2 until we tested gcc 4.1 on 30 June 2010 and got 1.21.
  54   VERIFY(error_avg < (NumTraits<Scalar>::IsComplex ? 8.0 : 1.25));
  55   VERIFY(error_max < (NumTraits<Scalar>::IsComplex ? 64.0 : 20.0));
  56
  57   {
  58     int s = 5;//internal::random<int>(4,10);
  59     int i = 0;//internal::random<int>(0,s-4);
  60     int j = 0;//internal::random<int>(0,s-4);
  61     Matrix<Scalar,5,5> mat(s,s);
  62     mat.setRandom();
  63     MatrixType submat = mat.template block<4,4>(i,j);
  64     MatrixType mat_inv = mat.template block<4,4>(i,j).inverse();
  65     VERIFY_IS_APPROX(mat_inv, submat.inverse());
  66     mat.template block<4,4>(i,j) = submat.inverse();
  67     VERIFY_IS_APPROX(mat_inv, (mat.template block<4,4>(i,j)));
  68   }
  69 }
  70
  71 void test_prec_inverse_4x4()
  72 {
  73   CALL_SUBTEST_1((inverse_permutation_4x4<Matrix4f>()));
  74   CALL_SUBTEST_1(( inverse_general_4x4<Matrix4f>(200000 * g_repeat) ));
  75   CALL_SUBTEST_1(( inverse_general_4x4<Matrix<float,4,4,RowMajor> >(200000 * g_repeat) ));
  76
  77   CALL_SUBTEST_2((inverse_permutation_4x4<Matrix<double,4,4,RowMajor> >()));
  78   CALL_SUBTEST_2(( inverse_general_4x4<Matrix<double,4,4,ColMajor> >(200000 * g_repeat) ));
  79   CALL_SUBTEST_2(( inverse_general_4x4<Matrix<double,4,4,RowMajor> >(200000 * g_repeat) ));
  80
  81   CALL_SUBTEST_3((inverse_permutation_4x4<Matrix4cf>()));
  82   CALL_SUBTEST_3((inverse_general_4x4<Matrix4cf>(50000 * g_repeat)));
  83 }