math/src/sqmatrix3.cc

   1 #include "../inc/sqmatrix3.h"
   2
   3 SqMatrix3::SqMatrix3(MatType type)
   4 {
   5     m_cols = new Vector3[m_SIZE];
   6     if (type != Mat_Null) {
   7         m_cols[0] = Vector3(Vector3::e1);
   8         m_cols[1] = Vector3(Vector3::e2);
   9         m_cols[2] = Vector3(Vector3::e3);
  10     }
  11 }
  12
  13 SqMatrix3::SqMatrix3(const SqMatrix3 &other)
  14 {
  15     m_cols = new Vector3[m_SIZE];
  16     _setCols(other.m_cols);
  17 }
  18
  19 SqMatrix3::SqMatrix3 (const Vector3 &col1, const Vector3 &col2, const Vector3& col3)
  20 {
  21     m_cols = new Vector3[m_SIZE];
  22     m_cols[0] = col1;
  23     m_cols[1] = col2;
  24     m_cols[2] = col3;
  25 }
  26
  27 SqMatrix3::~SqMatrix3()
  28 {
  29     delete[] m_cols;
  30     m_cols = 0;
  31 }
  32
  33 // operator =
  34 SqMatrix3& SqMatrix3::operator= (const SqMatrix3 &other)
  35 {
  36     _setCols(other.m_cols);
  37     return (*this);
  38 }
  39
  40 // arithmetic operators
  41 // +, +=
  42 SqMatrix3 SqMatrix3::operator+ (const SqMatrix3 &other) const
  43 {
  44    return SqMatrix3(*this) += other;
  45 }
  46
  47 SqMatrix3& SqMatrix3::operator+= (const SqMatrix3 &other)
  48 {
  49     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
  50         m_cols[i] += other.m_cols[i];
  51     }
  52     return (*this);
  53 }
  54
  55 // -, -=
  56 SqMatrix3 SqMatrix3::operator- (const SqMatrix3 &other) const
  57 {
  58     return SqMatrix3(*this) -= other;
  59 }
  60
  61 SqMatrix3& SqMatrix3::operator-= (const SqMatrix3 &other)
  62 {
  63     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
  64         m_cols[i] -= other.m_cols[i];
  65     }
  66     return (*this);
  67 }
  68
  69 // multiplications
  70 // matrix multiplication
  71
  72 #ifdef M_USE_STRASSEN
  73 SqMatrix3 SqMatrix3::operator* (const SqMatrix3 &other) const
  74 {
  75
  76 }
  77 #else // use naive algorithm
  78 SqMatrix3 SqMatrix3::operator* (const SqMatrix3 &other) const
  79 {
  80     SqMatrix3 result(Mat_Null);
  81
  82     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
  83         for (unsigned int j(0) ; j < m_SIZE ; ++j) {
  84             for (unsigned int k(0) ; k < m_SIZE ; ++k) {
  85                 result.at(i,j) += this->at(i,k) * other.at(k,j);
  86             }
  87         }
  88     }
  89
  90     return result;
  91 }
  92 #endif // M_USE_STRASSEN
  93
  94 SqMatrix3& SqMatrix3::operator*= (const SqMatrix3 &other)
  95 {
  96     return (*this = *this * other);
  97 }
  98
  99 // scalar multiplication
 100 SqMatrix3 SqMatrix3::operator* (double scalar) const
 101 {
 102     return SqMatrix3(*this) *= scalar;
 103 }
 104
 105 SqMatrix3& SqMatrix3::operator*= (double scalar)
 106 {
 107     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 108         m_cols[i] *= scalar;
 109     }
 110     return (*this);
 111 }
 112
 113 // matrix * vector3
 114 Vector3 SqMatrix3::operator* (const Vector3 &vector) const
 115 {
 116     // line vectors representing *this
 117     Vector3 *m_rows = new Vector3[m_SIZE];
 118     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 119         m_rows[i] = Vector3(this->at(i,0), this->at(i,1), this->at(i,2));
 120     }
 121
 122     // result is a vector3
 123     Vector3 result;
 124     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 125         result.at(i) = m_rows[i].dotProduct(vector, Vector3::EUCLID); // inner product
 126     }
 127
 128     delete[] m_rows;
 129     m_rows = 0;
 130
 131     return result;
 132 }
 133
 134 // comparison operator
 135 bool SqMatrix3::operator== (const SqMatrix3 &other) const
 136 {
 137     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 138         if (m_cols[i] != other.m_cols[i]) {
 139             return false;
 140         }
 141     }
 142     return true;
 143 }
 144
 145 bool SqMatrix3::operator!= (const SqMatrix3 &other) const
 146 {
 147     return !(*this == other);
 148 }
 149
 150 // transposition
 151 SqMatrix3 SqMatrix3::transpose() const
 152 {
 153     SqMatrix3 result(Mat_Null);
 154     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 155         result.m_cols[i] = Vector3(this->at(i,0), this->at(i,1), this->at(i,2));
 156     }
 157     return result;
 158 }
 159
 160 SqMatrix3& SqMatrix3::setToTransposed()
 161 {
 162     return ((*this) = this->transpose());
 163 }
 164
 165 // determinant (alias function det points directly to determinant, could be useful if we are lazy)
 166 double SqMatrix3::determinant() const
 167 {
 168     return (((this->at(0,0)) * ((this->at(1,1))*this->at(2,2) - (this->at(1,2))*(this->at(2,1))))
 169      - ((this->at(0,1)) * ((this->at(1,0))*this->at(2,2) - (this->at(1,2))*(this->at(2,0))))
 170      + ((this->at(0,2)) * ((this->at(1,0))*this->at(2,1) - (this->at(1,1))*(this->at(2,0)))));
 171 }
 172
 173 double SqMatrix3::det() const
 174 {
 175     return this->determinant();
 176 }
 177
 178 // inverse (direct formula)
 179 SqMatrix3 SqMatrix3::inverse() const
 180 {
 181     double det(this->determinant());
 182     if (det == 0.0) {
 183         throw std::domain_error("Tried to inverse matrix with null determinant");
 184     }
 185
 186     Vector3 col1((this->at(1,1))*(this->at(2,2)) - (this->at(1,2))*(this->at(2,1)),
 187             (this->at(1,2))*(this->at(2,0)) - (this->at(1,0))*(this->at(2,2)),
 188             (this->at(1,0))*(this->at(2,1)) - (this->at(1,1))*(this->at(2,0)));
 189     Vector3 col2((this->at(0,2))*(this->at(2,1)) - (this->at(0,1))*(this->at(2,2)),
 190             (this->at(0,0))*(this->at(2,2)) - (this->at(0,2))*(this->at(2,0)),
 191             (this->at(0,1))*(this->at(2,0)) - (this->at(0,0))*(this->at(2,1)));
 192     Vector3 col3((this->at(0,1))*(this->at(1,2)) - (this->at(0,2))*(this->at(1,1)),
 193             (this->at(0,2))*(this->at(1,0)) - (this->at(0,0))*(this->at(1,2)),
 194             (this->at(0,0))*(this->at(1,1)) - (this->at(0,1))*(this->at(1,0)));
 195
 196     SqMatrix3 result(col1, col2, col3);
 197     return (result * (1/det));
 198 }
 199
 200 SqMatrix3& SqMatrix3::setToInverse()
 201 {
 202     return (*this = this->inverse());
 203 }
 204
 205 // index operators : indexes are common math indexes, not C++ array indexes
 206 // setter (math-like)
 207 double& SqMatrix3::operator() (unsigned int row, unsigned int col)
 208 {
 209     if ((row > m_SIZE) or (col > m_SIZE)) {
 210         throw std::out_of_range("Passed ref index > 3 on 3 by 3 matrix");
 211     }
 212     else if ((row <= 0) or (col <= 0)) {
 213         throw std::out_of_range("Passed ref index 0 to SqMatrix3::operator()");
 214     }
 215
 216     return m_cols[col-1][row];
 217 }
 218
 219 // getter (math-like)
 220 double SqMatrix3::operator() (unsigned int row, unsigned int col) const
 221 {
 222     if ((row > m_SIZE) or (col > m_SIZE)) {
 223         throw std::out_of_range("Passed ref index > 3 on 3 by 3 matrix");
 224     }
 225     else if ((row <= 0) or (col <= 0)) {
 226         throw std::out_of_range("Passed ref index 0 to SqMatrix::operator()");
 227     }
 228
 229     return m_cols[col-1][row];
 230 }
 231
 232 //setter (C++-like)
 233 double& SqMatrix3::at (unsigned int row, unsigned int col)
 234 {
 235     if ((row >= m_SIZE) or (col >= m_SIZE)) {
 236         throw std::out_of_range("Passed ref index > 3 on 3 by 3 matrix");
 237     }
 238
 239     return m_cols[col].at(row);
 240 }
 241
 242 // getter (C++-like)
 243 double SqMatrix3::at (unsigned int row, unsigned int col) const
 244 {
 245     if ((row >= m_SIZE) or (col >= m_SIZE)) {
 246         throw std::out_of_range("Passed ref index > 3 on 3 by 3 matrix");
 247     }
 248
 249     return m_cols[col].at(row);
 250 }
 251
 252 // show matrix using cout by default
 253 void SqMatrix3::show(std::ostream &out) const
 254 {
 255     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 256         for (unsigned int j(0) ; j < m_SIZE ; ++j) {
 257             out << std::setfill(' ') << std::setw(6) << at(i,j) << " ";
 258         }
 259         out << std::endl;
 260     }
 261 }
 262
 263 // private methods
 264 // _setCols : copy operator for attribute m_cols
 265 void SqMatrix3::_setCols (const Vector3 *cols) // cols is intended to be a 3-dim array
 266 {
 267     for (unsigned int i(0) ; i < m_SIZE ; ++i) {
 268         m_cols[i] = cols[i];
 269     }
 270 }
 271
 272 // friend functions
 273 // external product (scalar product)
 274 SqMatrix3 operator* (double scalar, const SqMatrix3 &matrix)
 275 {
 276     return matrix * scalar;
 277 }
 278
 279 // operator<<
 280 std::ostream& operator<< (std::ostream &out, const SqMatrix3 &matrix)
 281 {
 282     matrix.show(out);
 283     return out;
 284 }
 285