core/vnl/algo/vnl_determinant.txx
Go to the documentation of this file.
00001 #ifndef vnl_algo_determinant_txx_
00002 #define vnl_algo_determinant_txx_
00003 /*
00004   fsm
00005 */
00006 #include "vnl_determinant.h"
00007 
00008 #include <vcl_cassert.h>
00009 #include <vnl/algo/vnl_qr.h>
00010 
00011 
00012 template <class T>
00013 T vnl_determinant(T const *row0, T const *row1)
00014 {
00015   return row0[0]*row1[1] - row0[1]*row1[0];
00016 }
00017 
00018 template <class T>
00019 T vnl_determinant(T const *row0, T const *row1, T const *row2)
00020 {
00021   return // the extra '+' makes it work nicely with emacs indentation.
00022     + row0[0]*row1[1]*row2[2]
00023     - row0[0]*row2[1]*row1[2]
00024     - row1[0]*row0[1]*row2[2]
00025     + row1[0]*row2[1]*row0[2]
00026     + row2[0]*row0[1]*row1[2]
00027     - row2[0]*row1[1]*row0[2];
00028 }
00029 
00030 template <class T>
00031 T vnl_determinant(T const *row0, T const *row1, T const *row2, T const *row3)
00032 {
00033   return
00034     + row0[0]*row1[1]*row2[2]*row3[3]
00035     - row0[0]*row1[1]*row3[2]*row2[3]
00036     - row0[0]*row2[1]*row1[2]*row3[3]
00037     + row0[0]*row2[1]*row3[2]*row1[3]
00038     + row0[0]*row3[1]*row1[2]*row2[3]
00039     - row0[0]*row3[1]*row2[2]*row1[3]
00040     - row1[0]*row0[1]*row2[2]*row3[3]
00041     + row1[0]*row0[1]*row3[2]*row2[3]
00042     + row1[0]*row2[1]*row0[2]*row3[3]
00043     - row1[0]*row2[1]*row3[2]*row0[3]
00044     - row1[0]*row3[1]*row0[2]*row2[3]
00045     + row1[0]*row3[1]*row2[2]*row0[3]
00046     + row2[0]*row0[1]*row1[2]*row3[3]
00047     - row2[0]*row0[1]*row3[2]*row1[3]
00048     - row2[0]*row1[1]*row0[2]*row3[3]
00049     + row2[0]*row1[1]*row3[2]*row0[3]
00050     + row2[0]*row3[1]*row0[2]*row1[3]
00051     - row2[0]*row3[1]*row1[2]*row0[3]
00052     - row3[0]*row0[1]*row1[2]*row2[3]
00053     + row3[0]*row0[1]*row2[2]*row1[3]
00054     + row3[0]*row1[1]*row0[2]*row2[3]
00055     - row3[0]*row1[1]*row2[2]*row0[3]
00056     - row3[0]*row2[1]*row0[2]*row1[3]
00057     + row3[0]*row2[1]*row1[2]*row0[3];
00058 }
00059 
00060 //--------------------------------------------------------------------------------
00061 
00062 template <class T>
00063 T vnl_determinant(vnl_matrix<T> const &M, bool balance)
00064 {
00065   unsigned n = M.rows();
00066   assert(M.cols() == n);
00067 
00068   switch (n)
00069   {
00070    case 1: return M[0][0];
00071    case 2: return vnl_determinant(M[0], M[1]);
00072    case 3: return vnl_determinant(M[0], M[1], M[2]);
00073    case 4: return vnl_determinant(M[0], M[1], M[2], M[3]);
00074    default:
00075     if (balance)
00076     {
00077       vnl_matrix<T> tmp(M);
00078       typedef typename vnl_numeric_traits<T>::abs_t abs_t;
00079       abs_t scalings(1);
00080       for (int t=0; t<5; ++t)
00081       {
00082         // normalize rows.
00083         for (unsigned int i=0; i<n; ++i) {
00084           abs_t rn = tmp.get_row(i).rms();
00085           if (rn > 0) {
00086             scalings *= rn;
00087             tmp.scale_row(i, abs_t(1)/rn);
00088           }
00089         }
00090         // normalize columns.
00091         for (unsigned int i=0; i<n; ++i) {
00092           abs_t rn = tmp.get_column(i).rms();
00093           if (rn > 0) {
00094             scalings *= rn;
00095             tmp.scale_column(i, abs_t(1)/rn);
00096           }
00097         }
00098 #if 0
00099         // pivot
00100         for (int k=0; k<n-1; ++k)
00101         {
00102           // find largest element after (k, k):
00103           int i0 = k, j0 = k;
00104           abs_t v0(0);
00105           for (int i=k; i<n; ++i) {
00106             for (int j=k; j<n; ++j) {
00107               abs_t v = vcl_abs(tmp[i][j]);
00108               if (v > v0) {
00109                 i0 = i;
00110                 j0 = j;
00111                 v0 = v;
00112               }
00113             }
00114           }
00115           // largest element is in position (i0, j0).
00116           if (i0 != k) {
00117             for (int j=0; j<n; ++j)
00118               vcl_swap(tmp[k][j], tmp[i0][j]);
00119             scalings = -scalings;
00120           }
00121           if (j0 != k) {
00122             for (int i=0; i<n; ++i)
00123               vcl_swap(tmp[i][k], tmp[i][j0]);
00124             scalings = -scalings;
00125           }
00126         }
00127 #endif
00128       }
00129       T balanced_det = vnl_qr<T>(tmp).determinant();
00130       //vcl_clog << __FILE__ ": scalings, balanced_det = " << scalings << ", " << balanced_det << vcl_endl;
00131       return T(scalings) * balanced_det;
00132     }
00133     else
00134       return vnl_qr<T>(M).determinant();
00135   }
00136 }
00137 
00138 
00139 //--------------------------------------------------------------------------------
00140 
00141 #define VNL_DETERMINANT_INSTANTIATE_1(T) \
00142 template T vnl_determinant(T const *, T const *); \
00143 template T vnl_determinant(T const *, T const *, T const *); \
00144 template T vnl_determinant(T const *, T const *, T const *, T const *)
00145 
00146 #define VNL_DETERMINANT_INSTANTIATE_2(T) \
00147 template T vnl_determinant(vnl_matrix<T > const &, bool)
00148 
00149 #undef VNL_DETERMINANT_INSTANTIATE
00150 #define VNL_DETERMINANT_INSTANTIATE(T) \
00151 VNL_DETERMINANT_INSTANTIATE_1(T); \
00152 VNL_DETERMINANT_INSTANTIATE_2(T)
00153 
00154 #endif // vnl_algo_determinant_txx_