Matd.cpp
Go to the documentation of this file.
00001 // ****************************************************************************
00002 // This file is part of the Integrating Vision Toolkit (IVT).
00003 //
00004 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
00005 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
00006 //
00007 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
00008 // All rights reserved.
00009 //
00010 // Redistribution and use in source and binary forms, with or without
00011 // modification, are permitted provided that the following conditions are met:
00012 //
00013 // 1. Redistributions of source code must retain the above copyright
00014 //    notice, this list of conditions and the following disclaimer.
00015 //
00016 // 2. Redistributions in binary form must reproduce the above copyright
00017 //    notice, this list of conditions and the following disclaimer in the
00018 //    documentation and/or other materials provided with the distribution.
00019 //
00020 // 3. Neither the name of the KIT nor the names of its contributors may be
00021 //    used to endorse or promote products derived from this software
00022 //    without specific prior written permission.
00023 //
00024 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
00025 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00026 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
00028 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00029 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00031 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00032 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00033 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034 // ****************************************************************************
00035 // ****************************************************************************
00036 // Filename:  Matd.cpp
00037 // Author:    Pedram Azad
00038 // Date:      2004
00039 // ****************************************************************************
00040 
00041 
00042 // ****************************************************************************
00043 // Includes
00044 // ****************************************************************************
00045 
00046 #include <new> // for explicitly using correct new/delete operators on VC DSPs
00047 
00048 #include "Vecd.h"
00049 #include "Matd.h"
00050 
00051 #include <math.h>
00052 #include <stdio.h>
00053 #include <string.h>
00054 
00055 
00056 
00057 // ****************************************************************************
00058 // Constructors / Destructor
00059 // ****************************************************************************
00060 
00061 CMatd::CMatd()
00062 {
00063         m_nRows = 0;
00064         m_nColumns = 0;
00065         m_ppElements = 0;
00066 }
00067 
00068 CMatd::CMatd(int nRows, int nColumns)
00069 {
00070         m_ppElements = 0;
00071 
00072         SetSize(nRows, nColumns);
00073 }
00074 
00075 CMatd::CMatd(const CMatd &m)
00076 {
00077         m_ppElements = 0;
00078 
00079         SetSize(m.m_nRows, m.m_nColumns);
00080 
00081         for (int i = 0; i < m_nRows; i++)
00082                 for (int j = 0; j < m_nColumns; j++)
00083                         m_ppElements[i][j] = m.m_ppElements[i][j];
00084 }
00085 
00086 CMatd::~CMatd()
00087 {
00088         if (m_ppElements)
00089         {
00090                 for (int i = 0; i < m_nRows; i++)
00091                         delete [] m_ppElements[i];
00092 
00093                 delete [] m_ppElements;
00094         }
00095 }
00096 
00097 
00098 // ****************************************************************************
00099 // Operators
00100 // ****************************************************************************
00101 
00102 double& CMatd::operator() (int nRow, int nColumn) const
00103 {
00104         //_ASSERTE(nRow >= 0 && nRow < m_nRows && nColumn >= 0 && nColumn < m_nColumns);
00105 
00106         return m_ppElements[nRow][nColumn];
00107 }
00108 
00109 CMatd& CMatd::operator= (const CMatd &m)
00110 {
00111         SetSize(m.m_nRows, m.m_nColumns);
00112 
00113         for (int i = 0; i < m_nRows; i++)
00114                 for (int j = 0; j < m_nColumns; j++)
00115                         m_ppElements[i][j] = m.m_ppElements[i][j];
00116 
00117         return *this;
00118 }
00119 
00120 void CMatd::operator*= (const double s)
00121 {
00122         for (int i = 0; i < m_nRows; i++)
00123                 for (int j = 0; j < m_nColumns; j++)
00124                         m_ppElements[i][j] *= s;
00125 }
00126 
00127 CMatd CMatd::operator* (const CMatd &m)
00128 {
00129         //_ASSERTE(m_nColumns == m.m_nRows && m_nColumns > 0);
00130 
00131         int i, j, k, newColumns = m.m_nColumns;
00132         double constantElement;
00133         CMatd result(m_nRows, newColumns);
00134 
00135         // ikj matrix multiplication
00136         for (i = 0; i < m_nRows; i++)
00137         {
00138                 constantElement = m_ppElements[i][0];
00139 
00140                 for (j = 0; j < newColumns; j++)
00141                 {
00142                         result.m_ppElements[i][j] = constantElement * m.m_ppElements[0][j];
00143                 }
00144         }
00145         for (i = 0; i < m_nRows; i++)
00146         {
00147                 for (k = 1; k < m_nColumns; k++)
00148                 {
00149                         constantElement = m_ppElements[i][k];
00150 
00151                         for (j = 0; j < newColumns; j++)                        
00152                         {
00153                                 result.m_ppElements[i][j] += constantElement * m.m_ppElements[k][j];
00154                         }
00155                 }
00156         }
00157 
00158         return result;
00159 }
00160 
00161 CMatd CMatd::operator+ (const CMatd &m)
00162 {
00163         if (m_nColumns != m.m_nColumns || m_nRows != m.m_nRows)
00164                 printf("error: incompatible input in CMatd::operator+(const CMatd&)\n");
00165 
00166         CMatd result(m_nRows, m_nColumns);
00167 
00168         // ikj matrix multiplication
00169         for (int i = 0; i < m_nRows; i++)
00170                 for (int j = 0; j < m_nColumns; j++)
00171                         result.m_ppElements[i][j] = m_ppElements[i][j] + m.m_ppElements[i][j];
00172 
00173         return result;
00174 }
00175 
00176 CMatd CMatd::operator- (const CMatd &m)
00177 {
00178         if (m_nColumns != m.m_nColumns || m_nRows != m.m_nRows)
00179                 printf("error: incompatible input in CMatd::operator-(const CMatd&)\n");
00180 
00181         CMatd result(m_nRows, m_nColumns);
00182 
00183         // ikj matrix multiplication
00184         for (int i = 0; i < m_nRows; i++)
00185                 for (int j = 0; j < m_nColumns; j++)
00186                         result.m_ppElements[i][j] = m_ppElements[i][j] - m.m_ppElements[i][j];
00187 
00188         return result;
00189 }
00190 
00191 CMatd CMatd::operator* (double s)
00192 {
00193         CMatd result(m_nRows, m_nColumns);
00194 
00195         // ikj matrix multiplication
00196         for (int i = 0; i < m_nRows; i++)
00197                 for (int j = 0; j < m_nColumns; j++)
00198                         result.m_ppElements[i][j] = s * m_ppElements[i][j];
00199 
00200         return result;
00201 }
00202 
00203 CVecd CMatd::operator* (const CVecd &v)
00204 {
00205         //_ASSERTE(m_nColumns == v.GetSize());
00206 
00207         CVecd result(m_nRows);
00208         
00209         for (int i = 0; i < m_nRows; i++)
00210         {
00211                 result[i] = 0.0;
00212 
00213                 for (int j = 0; j < m_nColumns; j++)
00214                 {
00215                         result[i] += m_ppElements[i][j] * v[j];
00216                 }
00217         }
00218 
00219         return result;
00220 }
00221 
00222 
00223 // ****************************************************************************
00224 // Methods
00225 // ****************************************************************************
00226 
00227 void CMatd::Zero()
00228 {
00229         for (int i = 0; i < m_nRows; i++)
00230                 for (int j = 0; j < m_nColumns; j++)
00231                         m_ppElements[i][j] = 0.0;
00232 }
00233 
00234 bool CMatd::Unit()
00235 {
00236         if (m_nRows != m_nColumns)
00237                 return false;
00238 
00239         Zero();
00240 
00241         for (int i = 0; i < m_nRows; i++)
00242                 m_ppElements[i][i] = 1.0;
00243 
00244 
00245         return true;
00246 }
00247 
00248 CMatd CMatd::Invert() const
00249 {
00250         if (m_nRows != m_nColumns)
00251         {
00252                 printf("error: input matrix must be square matrix for CMatd::Invert\n");
00253                 CMatd null(0, 0);
00254                 return null;
00255         }
00256 
00257         const int n = m_nColumns;
00258         int i;
00259 
00260         CMatd copiedMatrix(*this);
00261         CMatd resultMatrix(n, n);
00262 
00263         resultMatrix.Unit();
00264 
00265         int *pPivotRows = new int[n];
00266         for (i = 0; i < n; i++)
00267                 pPivotRows[i] = 0;
00268         
00269         // invert by gauss elimination
00270         for (i = 0; i < n; i++)
00271         {
00272                 int j, nPivotColumn = 0;
00273 
00274                 double *helper1 = copiedMatrix.m_ppElements[i];
00275                 double *helper2 = resultMatrix.m_ppElements[i];
00276 
00277                 // determine pivot element
00278                 double max = 0;
00279                 for (j = 0; j < n; j++)
00280                         if (fabs(helper1[j]) > max)
00281                         {
00282                                 max = fabs(helper1[j]);
00283                                 nPivotColumn = j;
00284                         }
00285 
00286                 pPivotRows[nPivotColumn] = i;
00287 
00288                 const double dPivotElement = copiedMatrix.m_ppElements[i][nPivotColumn];
00289 
00290                 if (fabs(dPivotElement) < 0.00001)
00291                 {
00292                         printf("error: input matrix is not regular for CMatd::Invert\n");
00293                         delete [] pPivotRows;
00294                         CMatd null(0, 0);
00295                         return null;
00296                 }
00297 
00298                 const double dFactor = 1.0 / dPivotElement;
00299                 
00300                 for (j = 0; j < n; j++)
00301                 {
00302                         helper1[j] *= dFactor;
00303                         helper2[j] *= dFactor;
00304                 }
00305 
00306                 for (j = 0; j < n; j++)
00307                 {
00308                         if (i != j)
00309                         {
00310                                 const double v = copiedMatrix.m_ppElements[j][nPivotColumn];
00311                                 int k;
00312 
00313                                 helper1 = copiedMatrix.m_ppElements[j];
00314                                 helper2 = copiedMatrix.m_ppElements[i];
00315                                 for (k = 0; k < n; k++)
00316                                         helper1[k] -= v * helper2[k];
00317                                 helper1[nPivotColumn] = 0; // explicitly set to zero
00318 
00319                                 helper1 = resultMatrix.m_ppElements[j];
00320                                 helper2 = resultMatrix.m_ppElements[i];
00321                                 for (k = 0; k < n; k++)
00322                                         helper1[k] -= v * helper2[k];
00323                         }
00324                 }
00325         }
00326 
00327         // bring result rows in pivot order
00328         double **ppTemp = new double*[n];
00329         for (i = 0; i < n; i++)
00330                 ppTemp[i] = resultMatrix.m_ppElements[i];
00331         
00332         for (i = 0; i < n; i++)
00333                 resultMatrix.m_ppElements[i] = ppTemp[pPivotRows[i]];
00334 
00335         delete [] pPivotRows;
00336 
00337         return resultMatrix;
00338 }
00339 
00340 void CMatd::SetSize(int nRows, int nColumns)
00341 {
00342         //_ASSERTE(nRows >= 0 && nColumns >= 0);
00343 
00344         // free memory first
00345         if (m_ppElements)
00346         {
00347                 for (int i = 0; i < m_nRows; i++)
00348                         delete [] m_ppElements[i];
00349                 delete [] m_ppElements;
00350         }
00351 
00352         // allocate memory for matrix
00353         m_ppElements = new double*[nRows];
00354         for (int i = 0; i < nRows; i++)
00355         {
00356                 m_ppElements[i] = new double[nColumns];
00357 
00358                 for (int j = 0; j < nColumns;  j++)
00359                         m_ppElements[i][j] = 0.0;
00360         }
00361 
00362         // save size of matrix
00363         m_nRows = nRows;
00364         m_nColumns = nColumns;
00365 }
00366 
00367 CMatd CMatd::GetTransposed()
00368 {
00369         CMatd result(m_nColumns, m_nRows);
00370 
00371         for (int i = 0; i < m_nRows; i++)
00372                 for (int j = 0; j < m_nColumns; j++)
00373                         result.m_ppElements[j][i] = m_ppElements[i][j];
00374 
00375         return result;
00376 }


asr_ivt
Author(s): Allgeyer Tobias, Hutmacher Robin, Kleinert Daniel, Meißner Pascal, Scholz Jonas, Stöckle Patrick
autogenerated on Thu Jun 6 2019 21:46:57