00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #include <qvmatrixalgebra.h>
00026 #include <QString>
00027 #include <QVMatrix>
00028
00030
00031
00032 QVMatrix::QVMatrix():
00033 cols(1), rows(1), data(new QBlasDataBuffer(cols*rows))
00034 { }
00035
00036 QVMatrix::QVMatrix(const QVMatrix &matrix):
00037 cols(matrix.cols), rows(matrix.rows), data(matrix.data)
00038 { }
00039
00040 QVMatrix::QVMatrix(const int rows, const int cols):
00041 cols(cols), rows(rows), data(new QBlasDataBuffer(cols*rows))
00042 { }
00043
00044 QVMatrix::QVMatrix(const QVQuaternion &quaternion): cols(4), rows(4), data(new QBlasDataBuffer(cols*rows))
00045 {
00046 *this = QVMatrix::identity(4);
00047
00048 operator()(0,0) += - 2.0 * (quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2]);
00049 operator()(0,1) += + 2.0 * (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
00050 operator()(0,2) += + 2.0 * (quaternion[2] * quaternion[0] + quaternion[1] * quaternion[3]);
00051
00052 operator()(1,0) += + 2.0 * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
00053 operator()(1,1) += - 2.0 * (quaternion[2] * quaternion[2] + quaternion[0] * quaternion[0]);
00054 operator()(1,2) += + 2.0 * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
00055
00056 operator()(2,0) += + 2.0 * (quaternion[2] * quaternion[0] - quaternion[1] * quaternion[3]);
00057 operator()(2,1) += + 2.0 * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
00058 operator()(2,2) += - 2.0 * (quaternion[1] * quaternion[1] + quaternion[0] * quaternion[0]);
00059 }
00060
00061 QVMatrix::QVMatrix(const QVVector &vector, const bool rowVector): cols(rowVector?vector.size():1), rows(rowVector?1:vector.size()), data(new QBlasDataBuffer(cols*rows))
00062 {
00063 if (rowVector)
00064 setRow(0,vector);
00065 else
00066 setCol(0,vector);
00067 }
00068
00069 QVMatrix::QVMatrix(const QList<QVVector> &vectorList): cols(vectorList.at(0).size()), rows(vectorList.size()), data(new QBlasDataBuffer(cols*rows))
00070 {
00071 for (int n = 0; n < getRows(); n++)
00072 {
00073 Q_ASSERT(vectorList.at(n).size() == getCols());
00074 setRow(n, vectorList.at(n));
00075 }
00076 }
00077
00078 QVMatrix::QVMatrix(const QList< QVector<double> > &vectorList): cols(vectorList.at(0).size()), rows(vectorList.size()), data(new QBlasDataBuffer(cols*rows))
00079 {
00080 for (int n = 0; n < getRows(); n++)
00081 {
00082 Q_ASSERT(vectorList.at(n).size() == getCols());
00083 setRow(n, vectorList.at(n));
00084 }
00085 }
00086
00087 QVMatrix::QVMatrix(const gsl_matrix *matrix): cols(matrix->size2), rows(matrix->size1), data(new QBlasDataBuffer(cols*rows))
00088 {
00089 for(int i = 0; i < rows; i++)
00090 for(int j = 0; j < cols; j++)
00091 operator()(i, j) = gsl_matrix_get(matrix, i, j);
00092 }
00093
00094 QVMatrix::QVMatrix(const QList<QPointF> &pointList): cols(2), rows(pointList.size()), data(new QBlasDataBuffer(cols*rows))
00095 {
00096 for (int n = 0; n < getRows(); n++)
00097 setRow(n, pointList.at(n));
00098 }
00099
00100 QVMatrix & QVMatrix::operator=(const QVMatrix &matrix)
00101 {
00102 cols = matrix.cols;
00103 rows = matrix.rows;
00104 data = matrix.data;
00105
00106 return *this;
00107 }
00108
00110
00111 QVVector QVMatrix::operator*(QVVector &vector) const
00112 {
00113 Q_ASSERT(vector.size() == getCols());
00114 return dotProduct(QVMatrix(vector, false)).getCol(0);
00115 }
00116
00118
00119 bool QVMatrix::equals(const QVMatrix &matrix) const
00120 {
00121
00122 if (cols != matrix.cols || rows == matrix.rows)
00123 return false;
00124
00125
00126 const double *data1 = getReadData(),
00127 *data2 = matrix.getReadData();
00128
00129 for (int i = 0; i < getDataSize(); i++)
00130 if (data1[i] != data2[i])
00131 return false;
00132
00133 return true;
00134 }
00135
00136 QVMatrix QVMatrix::dotProduct(const QVMatrix &matrix) const
00137 {
00138 const int cols1 = cols,
00139 rows1 = rows,
00140 cols2 = matrix.cols,
00141 rows2 = matrix.rows;
00142
00143 Q_ASSERT(cols1 == rows2);
00144
00145 QVMatrix result(rows1, cols2);
00146
00147 const int k = cols1, m = rows1, n = cols2;
00148
00149 cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
00150 m, n, k, 1.0,
00151 getReadData(), k,
00152 matrix.getReadData(), n, 0.0,
00153 result.getWriteData(), n);
00154
00155 return result;
00156 }
00157
00158 QVMatrix QVMatrix::matrixDivide(const QVMatrix &matrix) const
00159 {
00160 Q_ASSERT(matrix.getCols() >= matrix.getRows());
00161 Q_ASSERT(matrix.getCols() == (*this).getCols());
00162
00163 QVMatrix result(matrix.getRows(), (*this).getRows());
00164
00165 if (matrix.getCols() == matrix.getRows())
00166 solveLinear(matrix.transpose(), result, (*this).transpose());
00167 else
00168 solveOverDetermined(matrix.transpose(), result, (*this).transpose());
00169
00170 return result.transpose();
00171 }
00172
00173 QVMatrix QVMatrix::inverse() const
00174 {
00176 return pseudoInverse(*this);
00177 }
00178
00179 double QVMatrix::det() const
00180 {
00181 return determinant(*this);
00182 }
00183
00184 QVMatrix QVMatrix::transpose() const
00185 {
00186 const int rows = getRows(), cols = getCols();
00187
00188 QVMatrix result(cols, rows);
00189
00190 const double *matrixData = getReadData();
00191 double *resultData = result.getWriteData();
00192
00194 for (int i = 0; i < rows; i++)
00195 for (int j = 0; j < cols; j++)
00196 resultData[j*rows+i] = matrixData[i*cols+j];
00197
00198 return result;
00199 }
00200
00201 void QVMatrix::set(const double value)
00202 {
00203 double *resultData = getWriteData();
00204 const int dataSize = getDataSize();
00205
00207
00208 for (int i = 0; i < dataSize; i++)
00209 resultData[i] = 0;
00210 }
00211
00212 QVMatrix QVMatrix::addition(const QVMatrix &matrix) const
00213 {
00214 Q_ASSERT(cols == matrix.cols || rows == matrix.rows);
00215
00216 QVMatrix result = *this;
00217
00218 const double *matrixData = matrix.getReadData();
00219 double *resultData = result.getWriteData();
00220 const int dataSize = matrix.getDataSize();
00221
00223
00224 for (int i = 0; i < dataSize; i++)
00225 resultData[i] += matrixData[i];
00226
00227 return result;
00228 }
00229
00230 QVMatrix QVMatrix::substract(const QVMatrix &matrix) const
00231 {
00232
00233 Q_ASSERT(cols == matrix.cols || rows == matrix.rows);
00234
00235 QVMatrix result = *this;
00236
00237 const double *matrixData = matrix.getReadData();
00238 double *resultData = result.getWriteData();
00239 const int dataSize = matrix.getDataSize();
00240
00242
00243 for (int i = 0; i < dataSize; i++)
00244 resultData[i] -= matrixData[i];
00245
00246 return result;
00247 }
00248
00249 QVMatrix QVMatrix::scalarProduct(const double value) const
00250 {
00251 QVMatrix result = *this;
00252
00253 double *resultData = result.getWriteData();
00254 const int dataSize = getDataSize();
00255
00257
00258 for (int i = 0; i < dataSize; i++)
00259 resultData[i] *= value;
00260
00261 return result;
00262 }
00263
00264 QVMatrix QVMatrix::scalarDivide(const double value) const
00265 {
00266 QVMatrix result = *this;
00267
00268 double *resultData = result.getWriteData();
00269 const int dataSize = getDataSize();
00270
00272
00273 for (int i = 0; i < dataSize; i++)
00274 resultData[i] /= value;
00275
00276 return result;
00277 }
00278
00279 double QVMatrix::norm2() const
00280 { return cblas_dnrm2(getDataSize(), getReadData(), 1); };
00281
00282
00283 QVMatrix QVMatrix::rowHomogeneousNormalize() const
00284 {
00285 const int cols = getCols(), rows = getRows();
00286 QVMatrix result(rows, cols);
00287
00288 for (int i = 0; i < rows; i++)
00289 for (int j = 0; j < cols; j++)
00290 result(i,j) = operator()(i,j) / operator()(i,cols-1);
00291 return result;
00292 }
00293
00294 const QVVector QVMatrix::getRow(const int row) const
00295 {
00296 Q_ASSERT(row < getRows());
00297
00298 const int cols = getCols();
00299 QVVector result(cols);
00300 for (int col= 0; col < cols; col++)
00301 result[col] = operator()(row,col);
00302
00303 return result;
00304 }
00305
00306 void QVMatrix::setRow(const int row, QVVector vector)
00307 {
00308 Q_ASSERT(row < getRows());
00309 Q_ASSERT(getCols() == vector.size());
00310
00311 const int cols = getCols();
00312 for (int col= 0; col < cols; col++)
00313 operator()(row,col) = vector[col];
00314 }
00315
00316 void QVMatrix::setRow(const int row, QVector<double> vector)
00317 {
00318 Q_ASSERT(row < getRows());
00319 Q_ASSERT(getCols() == vector.size());
00320
00321 const int cols = getCols();
00322 for (int col= 0; col < cols; col++)
00323 operator()(row,col) = vector[col];
00324 }
00325
00326 const QVVector QVMatrix::getCol(const int col) const
00327 {
00328 Q_ASSERT(col < getCols());
00329
00330 const int rows = getRows();
00331 QVVector result(rows);
00332 for (int row= 0; row < rows; row++)
00333 result[row] = operator()(row,col);
00334
00335 return result;
00336 }
00337
00338 void QVMatrix::setCol(const int col, QVVector vector)
00339 {
00340 Q_ASSERT(col < getCols());
00341 Q_ASSERT(getRows() == vector.size());
00342
00343 const int rows = getRows();
00344 for (int row= 0; row < rows; row++)
00345 operator()(row,col) = vector[row];
00346 }
00347
00348 const QVMatrix QVMatrix::getSubmatrix(const int firstRow, const int lastRow, const int firstCol, const int lastCol) const
00349 {
00350 const int rows = getRows(), cols = getCols();
00351
00352 Q_ASSERT(firstRow >= 0);
00353 Q_ASSERT(firstCol >= 0);
00354 Q_ASSERT(firstRow <= lastRow);
00355 Q_ASSERT(firstCol <= lastCol);
00356 Q_ASSERT(lastRow < rows);
00357 Q_ASSERT(lastCol < cols);
00358
00359 QVMatrix result(lastRow - firstRow +1, lastCol - firstCol +1);
00360
00361 for (int row = firstRow; row <= lastRow; row++)
00362 for (int col = firstCol; col < lastCol; col++)
00363 result(row - firstRow, col - firstCol) = operator()(row,col);
00364
00365 return result;
00366 }
00367
00369
00370
00371 QVMatrix QVMatrix::identity(const int size)
00372 {
00373 QVMatrix result(size, size);
00374 result.set(0);
00375 for (int i= 0; i < size; i++)
00376 result(i,i) = 1;
00377 return result;
00378 }
00379
00380 QVMatrix QVMatrix::random(const int rows, const int cols)
00381 {
00382 QVMatrix result(rows, cols);
00383 for (int col = 0; col < cols; col++)
00384 for (int row = 0; row < rows; row++)
00385 result(row,col) = (double)ABS(rand()) / (double)std::numeric_limits<int>::max();
00386 return result;
00387 }
00388
00389 QVMatrix QVMatrix::diagonal(const QVVector &diagonalVector)
00390 {
00391 const int size = diagonalVector.size();
00392 QVMatrix result(size, size);
00393 result.set(0);
00394 for (int i= 0; i < size; i++)
00395 result(i,i) = diagonalVector[i];
00396 return result;
00397 }
00398
00399 QVMatrix QVMatrix::rotationMatrix(const double delta)
00400 {
00401
00402 QVMatrix result = QVMatrix::identity(3);
00403
00404 result(0,0) = cos(delta); result(0,1) = sin(delta);
00405 result(1,0) = -sin(delta); result(1,1) = cos(delta);
00406
00407 return result;
00408 }
00409
00410 QVMatrix QVMatrix::traslationMatrix(const double x, const double y)
00411
00412 {
00413 QVMatrix result = QVMatrix::identity(3);
00414
00415 result(0,2) = x;
00416 result(1,2) = y;
00417
00418 return result;
00419 }
00420
00421 QVMatrix QVMatrix::scaleMatrix(const double zoom)
00422 {
00423 QVMatrix result = QVMatrix::identity(3);
00424
00425 result(0,0) = zoom;
00426 result(1,1) = zoom;
00427
00428 return result;
00429 }
00430
00431 QVMatrix QVMatrix::rotationMatrix3dZAxis(const double angle)
00432 {
00433 QVMatrix result = identity(4);
00434
00435 result(0,0) = cos(angle);
00436 result(0,1) = sin(angle);
00437
00438 result(1,0) = -sin(angle);
00439 result(1,1) = cos(angle);
00440
00441 return result;
00442 }
00443
00444 QVMatrix QVMatrix::rotationMatrix3dXAxis(const double angle)
00445 {
00446 QVMatrix result = identity(4);
00447
00448 result(1,1) = cos(angle);
00449 result(1,2) = sin(angle);
00450
00451 result(2,1) = -sin(angle);
00452 result(2,2) = cos(angle);
00453
00454 return result;
00455 }
00456
00457 QVMatrix QVMatrix::rotationMatrix3dYAxis(const double angle)
00458 {
00459 QVMatrix result = identity(4);
00460
00461 result(0,0) = cos(angle);
00462 result(0,2) = -sin(angle);
00463
00464 result(2,0) = sin(angle);
00465 result(2,2) = cos(angle);
00466
00467 return result;
00468 }
00469
00470 QVMatrix QVMatrix::traslationMatrix3d(const double x, const double y, const double z)
00471 {
00472 QVMatrix result = identity(4);
00473
00474 result(3,0) = x;
00475 result(3,1) = y;
00476 result(3,2) = z;
00477
00478 return result;
00479 }
00480
00481 QVVector QVMatrix::meanCol() const
00482 {
00483 QVVector result = getCol(0);
00484 for (int i = 1; i < getCols(); i++)
00485 result += getCol(i);
00486 return result / getCols();
00487 }
00488
00490
00491 std::ostream& operator << ( std::ostream &os, const QVMatrix &matrix )
00492 {
00493 const int cols = matrix.getCols(), rows = matrix.getRows();
00494
00495 os << "QVMatrix (" << rows << ", " << cols << ")" << std::endl;
00496
00497 const double *data = matrix.getReadData();
00498 os << "[" << std::endl;
00499 for (int i=0; i < rows; i++)
00500 {
00501 os << " [ ";
00502 for (int j = 0; j < cols; j++)
00503 os << qPrintable(QString("%1").arg(data[i*cols + j], -8, 'f', 6)) << " ";
00504 os << "]" << std::endl;
00505 }
00506 os << "]" << std::endl;
00507 return os;
00508 }
00509
00510 std::istream& operator >> ( std::istream &is, QVMatrix &matrix )
00511 {
00512 int cols, rows;
00513 double value;
00514
00515 is.ignore(256, '(');
00516 is >> rows;
00517 is.ignore(256, ',');
00518 is >> cols;
00519 is.ignore(256, '[');
00520 QVMatrix maux(rows, cols);
00521
00522 for (int i = 0; i < rows; i++)
00523 {
00524 is.ignore(256, '[');
00525 for (int j = 0; j < cols; j++)
00526 {
00527 is >> value;
00528 maux(i, j) = value;
00529 }
00530 }
00531
00532 matrix = maux;
00533
00534 return is;
00535 }
00536
00537 uint qHash(const QVMatrix &matrix)
00538 {
00539 const int cols = matrix.getCols(), rows = matrix.getRows();
00540
00541 double accum = 0;
00542 for (int i = 0; i < cols; i++)
00543 for (int j = 0; j < rows; j++)
00544 accum += matrix(i,j) / matrix(cols-i-1, rows-j-1);
00545
00546 return (uint) ((100000 * accum) / ((double) (cols + rows)));
00547 }
00548
00549