SmallMatrixOpts.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 //
00004 // Inverse of 2-matrix
00005 // Must be invertible!
00006 #ifndef __SMALL_MATRIX_OPTS
00007 #define __SMALL_MATRIX_OPTS
00008 #include <cassert>
00009 
00010 inline Matrix<2> M2Inverse(const Matrix<2> &m)
00011 {
00012   Matrix<2> m2Res;
00013   double dDet = m[0][0] * m[1][1] - m[1][0] * m[0][1];
00014   assert(dDet!=0.0);
00015   double dInverseDet = 1.0 / dDet;
00016   m2Res[0][0] = m[1][1] * dInverseDet;
00017   m2Res[1][1] = m[0][0] * dInverseDet;
00018   m2Res[1][0] = -m[1][0] * dInverseDet;
00019   m2Res[0][1] = -m[0][1] * dInverseDet;
00020   return m2Res;
00021 };
00022 
00023 // Determinant of 2x2
00024 inline double M2Det(Matrix<2> m)
00025 {
00026   return m[0][0] * m[1][1]  - m[0][1] * m[1][0];
00027 }
00028 
00029 // Determinant of 3x3
00030 inline double M3Det(Matrix<3> m )
00031 {
00032   return  
00033     m[0][0] * (m[1][1] * m[2][2]  - m[1][2] * m[2][1]) - 
00034     m[0][1] * (m[1][0] * m[2][2]  - m[1][2] * m[2][0]) + 
00035     m[0][2] * (m[1][0] * m[2][1]  - m[1][1] * m[2][0]);
00036 }
00037 
00038 #endif


tum_ardrone
Author(s):
autogenerated on Sat Jun 8 2019 20:27:23