squarematrix.h
Go to the documentation of this file.
1 
9 #ifndef SQUARE_MATRIX_H
10 #define SQUARE_MATRIX_H
11 
12 #include <argos3/core/utility/math/matrix/matrix.h>
13 
14 namespace argos {
15 
16  template <UInt32 DIM>
17  class CSquareMatrix : public CMatrix<DIM, DIM> {
18 
19  public:
20  CSquareMatrix() : CMatrix<DIM, DIM>() {}
21 
22  CSquareMatrix(const CMatrix<DIM, DIM>& c_other) :
23  CMatrix<DIM, DIM>(c_other) {}
24 
26  for(UInt32 i = 0; i < DIM; i++) {
27  for(UInt32 j = 0; j < DIM; j++) {
28  if(i == j) {
29  CMatrix<DIM, DIM>::m_pfValues[i * DIM + j] = 1;
30  }
31  else {
32  CMatrix<DIM, DIM>::m_pfValues[i * DIM + j] = 0;
33  }
34  }
35  }
36  }
37 
39  bool bIsIdentMat = true;
40  for(UInt32 i = 0; i < DIM; i++) {
41  for(UInt32 j = 0; j < DIM; j++) {
42  if(i == j) {
43  if(CMatrix<DIM, DIM>::m_pfValues[i * DIM + j] != 1) {
44  bIsIdentMat = false;
45  break;
46  }
47  }
48  else {
49  if(CMatrix<DIM, DIM>::m_pfValues[i * DIM + j] != 0) {
50  bIsIdentMat = false;
51  break;
52  }
53  }
54  }
55  }
56  return bIsIdentMat;
57  }
58 
59  /*
60  Calculate the determinant
61  */
62  Real GetDeterminant() const {
63  Real fDet = 0.0;
64  CSquareMatrix<DIM - 1> cTempMatrix;
65  for(UInt32 unColTempA = 0; unColTempA < DIM; unColTempA++) {
66  for(UInt32 unRow = 1; unRow < DIM; unRow++) {
67  UInt32 unColTempB = 0;
68  for (UInt32 unCol = 0; unCol < DIM; unCol++) {
69  if (unCol == unColTempA)
70  continue;
71  cTempMatrix(unRow - 1, unColTempB) = CMatrix<DIM, DIM>::m_pfValues[unRow * DIM + unCol];
72  unColTempB++;
73  }
74  }
75  fDet += std::pow(-1.0, unColTempA + 2u) * CMatrix<DIM, DIM>::m_pfValues[unColTempA] * cTempMatrix.GetDeterminant();
76  }
77  return fDet;
78  }
79 
84  CSquareMatrix<DIM> cCofactorMatrix;
85  CSquareMatrix<DIM - 1> cTempMatrix;
86  for(UInt32 unCofactorCol = 0; unCofactorCol < DIM; unCofactorCol++) {
87  for(UInt32 unCofactorRow = 0; unCofactorRow < DIM; unCofactorRow++) {
88  /* Form the adjoint */
89  UInt32 unTempRow = 0;
90  for(UInt32 unRow = 0; unRow < DIM; unRow++) {
91  if(unRow != unCofactorRow) {
92  UInt32 unTempCol = 0;
93  for(UInt32 unCol = 0; unCol < DIM; unCol++) {
94  if(unCol != unCofactorCol) {
95  cTempMatrix(unTempRow, unTempCol) = CMatrix<DIM, DIM>::m_pfValues[unRow * DIM + unCol];
96  unTempCol++;
97  }
98  }
99  unTempRow++;
100  }
101  }
102  /* Fill in the elements of the cofactor matrix */
103  cCofactorMatrix(unCofactorRow, unCofactorCol) =
104  std::pow(-1.0, unCofactorRow + unCofactorCol + 2u) * cTempMatrix.GetDeterminant();
105  }
106  }
107  return cCofactorMatrix;
108  }
109 
111  CSquareMatrix<DIM> cTransposedMatrix;
112  for(UInt32 i = 0; i < DIM; i++) {
113  for(UInt32 j = 0; j < DIM; j++) {
114  cTransposedMatrix(j, i) = CMatrix<DIM, DIM>::m_pfValues[i * DIM + j];
115  }
116  }
117  return cTransposedMatrix;
118  }
119 
121  CSquareMatrix<DIM> cInverseMatrix = GetCofactorMatrix().GetTransposed();
122  cInverseMatrix *= 1.0 / GetDeterminant();
123  return cInverseMatrix;
124  }
125 
126  };
127 
128  template<>
130 
131  template<>
133 
134  template<>
135  CSquareMatrix<2> CSquareMatrix<2>::GetCofactorMatrix() const;
136 }
137 
138 #endif
Real GetDeterminant() const
Definition: squarematrix.h:62
CSquareMatrix< DIM > GetInverse() const
Definition: squarematrix.h:120
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
CSquareMatrix(const CMatrix< DIM, DIM > &c_other)
Definition: squarematrix.h:22
CSquareMatrix< DIM > GetTransposed() const
Definition: squarematrix.h:110
CSquareMatrix< DIM > GetCofactorMatrix() const
Find the cofactor matrix.
Definition: squarematrix.h:83
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12