core/vgui/internals/vgui_invert_homg4x4.cxx Source File

Go to the documentation of this file.
00001 // This is core/vgui/internals/vgui_invert_homg4x4.cxx
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005 //:
00006 // \file
00007 // \author fsm
00008 
00009 #include "vgui_invert_homg4x4.h"
00010 
00011 static bool is_diagonal(double const * const *M)
00012 {
00013   for (unsigned i=0; i<4; ++i)
00014     for (unsigned j=0; j<4; ++j)
00015       if ( (i != j) && (M[i][j] != 0) )
00016         return false;
00017   return true;
00018 }
00019 
00020 // Translation plus scale
00021 static bool is_trans_scale(double const * const *M)
00022 {
00023   return                  M[0][1] == 0 && M[0][2] == 0 &&
00024           M[1][0] == 0 &&                 M[1][2] == 0 &&
00025           M[2][0] == 0 && M[2][1] == 0 &&
00026           M[3][0] == 0 && M[3][1] == 0 && M[3][2] == 0;
00027 }
00028 
00029 // return pointer to static data
00030 static double const * const * detA_inverseA(double const * const *A/*4x4*/)
00031 {
00032   static double data[4][4]={
00033     { A[1][1]*A[2][2]*A[3][3]-A[1][1]*A[2][3]*A[3][2]-A[2][1]*A[1][2]*A[3][3]
00034      +A[2][1]*A[1][3]*A[3][2]+A[3][1]*A[1][2]*A[2][3]-A[3][1]*A[1][3]*A[2][2],
00035      -A[0][1]*A[2][2]*A[3][3]+A[0][1]*A[2][3]*A[3][2]+A[2][1]*A[0][2]*A[3][3]
00036      -A[2][1]*A[0][3]*A[3][2]-A[3][1]*A[0][2]*A[2][3]+A[3][1]*A[0][3]*A[2][2],
00037       A[0][1]*A[1][2]*A[3][3]-A[0][1]*A[1][3]*A[3][2]-A[1][1]*A[0][2]*A[3][3]
00038      +A[1][1]*A[0][3]*A[3][2]+A[3][1]*A[0][2]*A[1][3]-A[3][1]*A[0][3]*A[1][2],
00039      -A[0][1]*A[1][2]*A[2][3]+A[0][1]*A[1][3]*A[2][2]+A[1][1]*A[0][2]*A[2][3]
00040      -A[1][1]*A[0][3]*A[2][2]-A[2][1]*A[0][2]*A[1][3]+A[2][1]*A[0][3]*A[1][2] },
00041     {-A[1][0]*A[2][2]*A[3][3]+A[1][0]*A[2][3]*A[3][2]+A[2][0]*A[1][2]*A[3][3]
00042      -A[2][0]*A[1][3]*A[3][2]-A[3][0]*A[1][2]*A[2][3]+A[3][0]*A[1][3]*A[2][2],
00043       A[0][0]*A[2][2]*A[3][3]-A[0][0]*A[2][3]*A[3][2]-A[2][0]*A[0][2]*A[3][3]
00044      +A[2][0]*A[0][3]*A[3][2]+A[3][0]*A[0][2]*A[2][3]-A[3][0]*A[0][3]*A[2][2],
00045      -A[0][0]*A[1][2]*A[3][3]+A[0][0]*A[1][3]*A[3][2]+A[1][0]*A[0][2]*A[3][3]
00046      -A[1][0]*A[0][3]*A[3][2]-A[3][0]*A[0][2]*A[1][3]+A[3][0]*A[0][3]*A[1][2],
00047       A[0][0]*A[1][2]*A[2][3]-A[0][0]*A[1][3]*A[2][2]-A[1][0]*A[0][2]*A[2][3]
00048      +A[1][0]*A[0][3]*A[2][2]+A[2][0]*A[0][2]*A[1][3]-A[2][0]*A[0][3]*A[1][2] },
00049     { A[1][0]*A[2][1]*A[3][3]-A[1][0]*A[2][3]*A[3][1]-A[2][0]*A[1][1]*A[3][3]
00050      +A[2][0]*A[1][3]*A[3][1]+A[3][0]*A[1][1]*A[2][3]-A[3][0]*A[1][3]*A[2][1],
00051      -A[0][0]*A[2][1]*A[3][3]+A[0][0]*A[2][3]*A[3][1]+A[2][0]*A[0][1]*A[3][3]
00052      -A[2][0]*A[0][3]*A[3][1]-A[3][0]*A[0][1]*A[2][3]+A[3][0]*A[0][3]*A[2][1],
00053       A[0][0]*A[1][1]*A[3][3]-A[0][0]*A[1][3]*A[3][1]-A[1][0]*A[0][1]*A[3][3]
00054      +A[1][0]*A[0][3]*A[3][1]+A[3][0]*A[0][1]*A[1][3]-A[3][0]*A[0][3]*A[1][1],
00055      -A[0][0]*A[1][1]*A[2][3]+A[0][0]*A[1][3]*A[2][1]+A[1][0]*A[0][1]*A[2][3]
00056      -A[1][0]*A[0][3]*A[2][1]-A[2][0]*A[0][1]*A[1][3]+A[2][0]*A[0][3]*A[1][1] },
00057     {-A[1][0]*A[2][1]*A[3][2]+A[1][0]*A[2][2]*A[3][1]+A[2][0]*A[1][1]*A[3][2]
00058      -A[2][0]*A[1][2]*A[3][1]-A[3][0]*A[1][1]*A[2][2]+A[3][0]*A[1][2]*A[2][1],
00059       A[0][0]*A[2][1]*A[3][2]-A[0][0]*A[2][2]*A[3][1]-A[2][0]*A[0][1]*A[3][2]
00060      +A[2][0]*A[0][2]*A[3][1]+A[3][0]*A[0][1]*A[2][2]-A[3][0]*A[0][2]*A[2][1],
00061      -A[0][0]*A[1][1]*A[3][2]+A[0][0]*A[1][2]*A[3][1]+A[1][0]*A[0][1]*A[3][2]
00062      -A[1][0]*A[0][2]*A[3][1]-A[3][0]*A[0][1]*A[1][2]+A[3][0]*A[0][2]*A[1][1],
00063       A[0][0]*A[1][1]*A[2][2]-A[0][0]*A[1][2]*A[2][1]-A[1][0]*A[0][1]*A[2][2]
00064      +A[1][0]*A[0][2]*A[2][1]+A[2][0]*A[0][1]*A[1][2]-A[2][0]*A[0][2]*A[1][1] }
00065   };
00066   static double *out[4] = {data[0], data[1], data[2], data[3]};
00067   return out;
00068 }
00069 
00070 bool vgui_invert_homg4x4(double const * const *M, double **Mi)
00071 {
00072   if (is_diagonal(M)) {
00073     // * 0 0 0
00074     // 0 * 0 0
00075     // 0 0 * 0
00076     // 0 0 0 *
00077     // Assume diagonal elements are non-zero.
00078     for (unsigned i=0; i<4; ++i)
00079       for (unsigned j=0; j<4; ++j)
00080         Mi[i][j] = (i==j ? 1.0/M[i][i] : 0);
00081     return true;
00082   }
00083 
00084   if (is_trans_scale(M)) {
00085     // * 0 0 *
00086     // 0 * 0 *
00087     // 0 0 * *
00088     // 0 0 0 *
00089     Mi[0][0] = 1/M[0][0]; Mi[0][1] = 0;         Mi[0][2] = 0;         Mi[0][3] = -M[0][3]/M[0][0];
00090     Mi[1][0] = 0;         Mi[1][1] = 1/M[1][1]; Mi[1][2] = 0;         Mi[1][3] = -M[1][3]/M[1][1];
00091     Mi[2][0] = 0;         Mi[2][1] = 0;         Mi[2][2] = 1/M[2][2]; Mi[2][3] = -M[2][3]/M[2][2];
00092     Mi[3][0] = 0;         Mi[3][1] = 0;         Mi[3][2] = 0;         Mi[3][3] =        1/M[3][3];
00093     return true;
00094   }
00095 
00096   // add more easy cases here....
00097 
00098   // closed-form inversion.
00099   // may be much faster.
00100   // may be less accurate.
00101   // if the given matrix has rank 0,1 or 2, the computed inverse will be zero.
00102   // if the given matrix has rank 3, the computed inverse will have rank 1.
00103   // else, the computed matrix should have rank 4.
00104   double const * const *out = detA_inverseA(M); // = vnl_inverse(M)
00105   for (unsigned i=0; i<4; ++i)
00106     for (unsigned j=0; j<4; ++j)
00107       Mi[i][j] = out[i][j];
00108 
00109   return true;
00110 }
00111 
00112 bool vgui_invert_homg4x4(double const A[4][4], double B[4][4])
00113 {
00114   double const *A_[4] = {A[0], A[1], A[2], A[3]};
00115   double       *B_[4] = {B[0], B[1], B[2], B[3]};
00116   return vgui_invert_homg4x4(A_, B_);
00117 }