SimpleMatrix operator * (double a, const SiconosMatrix & A)
{
// To compute B = a * A
unsigned int numA = A.getNum();
if (numA == 6) // if A = 0
{
//DenseMat p(zero_matrix(A.size(0),A.size(1)));
//return p;
return A;
}
else if (numA == 7)
{
return (DenseMat)(a**A.identity());
}
else if (numA == 0) // A block
{
SimpleMatrix tmp(A); // ... copy ...
tmp *= a;
return tmp;
}
else if (numA == 1) // dense)
return (DenseMat)(a** A.dense());
else if (numA == 2)
return (TriangMat)(a ** A.triang());
else if (numA == 3)
return (SymMat)(a ** A.sym());
else if (numA == 4)
return (SparseMat)(a ** A.sparse());
else //if(numA==5)
return (BandedMat)(a ** A.banded());
}
const SimpleMatrix operator / (const SiconosMatrix & A, double a)
{
// To compute B = A/a
if (a == 0.0)
SiconosMatrixException::selfThrow(" Matrix, operator / , division by zero.");
unsigned int numA = A.getNum();
if (numA == 6) // if A = 0
{
//DenseMat p(zero_matrix(A.size(0),A.size(1)));
//return p;
return A;
}
else if (numA == 7)
{
return (DenseMat)(*A.identity() / a);
}
else if (numA == 0) // A block
{
SimpleMatrix tmp(A); // ... copy ...
tmp /= a;
return tmp;
}
else if (numA == 1) // dense)
return (DenseMat)(*A.dense() / a);
else if (numA == 2)
return (TriangMat)(*A.triang() / a);
else if (numA == 3)
return (SymMat)(*A.sym() / a);
else if (numA == 4)
return (SparseMat)(*A.sparse() / a);
else //if(numA==5)
return (BandedMat)(*A.banded() / a);
}
void add(const SiconosMatrix & A, const SiconosMatrix& B, SiconosMatrix& C)
{
// To compute C = A + B in an "optimized" way (in comparison with operator +)
if ((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
SiconosMatrixException::selfThrow("Matrix addition: inconsistent sizes");
if ((A.size(0) != C.size(0)) || (A.size(1) != C.size(1)))
SiconosMatrixException::selfThrow("Matrix addition: inconsistent sizes");
unsigned int numA = A.getNum();
unsigned int numB = B.getNum();
unsigned int numC = C.getNum();
// === if C is zero or identity => read-only ===
if (numC == 6 || numC == 7)
SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");
// === common memory between A, B, C ===
if (&A == &C) // A and C have common memory
{
C += B;
}
else if (&B == &C) // B and C have common memory
{
C += A;
}
else // No common memory between C and A or B.
{
if (numA == 6) // A = 0
C = B ;
else if (numB == 6) // B = 0
C = A;
else // A and B different from 0
{
if (numC == 0) // if C is Block
{
if (numA != 0) // A simple, whatever is B
{
C = A;
C += B;
}
else // A Block
{
C = B;
C += A;
}
}
else // if C is a SimpleMatrix
{
if (numA == numB && numA != 0) // A and B are of the same type and NOT block
{
if (numC == numA)
{
if (numA == 1)
noalias(*C.dense()) = *A.dense() + *B.dense();
else if (numA == 2)
noalias(*C.triang()) = *A.triang() + *B.triang();
else if (numA == 3)
noalias(*C.sym()) = *A.sym() + *B.sym();
else if (numA == 4)
noalias(*C.sparse()) = *A.sparse() + *B.sparse();
else //if(numA==5)
noalias(*C.banded()) = *A.banded() + *B.banded();
}
else // C and A of different types.
{
if (numC != 1)
SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
// Only dense matrices are allowed for output.
if (numA == 1)
noalias(*C.dense()) = *A.dense() + *B.dense();
else if (numA == 2)
noalias(*C.dense()) = *A.triang() + *B.triang();
else if (numA == 3)
noalias(*C.dense()) = *A.sym() + *B.sym();
else if (numA == 4)
noalias(*C.dense()) = *A.sparse() + *B.sparse();
else //if(numA==5)
noalias(*C.dense()) = *A.banded() + *B.banded();
}
C.resetLU();
}
else if (numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
{
if (numC != 1)
SiconosMatrixException::selfThrow("Matrix addition ( add(A,B,C) ): wrong type for resulting matrix C.");
// Only dense matrices are allowed for output.
if (numA == 1)
switch (numB)
{
case 2:
noalias(*C.dense()) = *A.dense() + *B.triang();
break;
case 3:
noalias(*C.dense()) = *A.dense() + *B.sym();
break;
case 4:
noalias(*C.dense()) = *A.dense() + *B.sparse();
break;
case 5:
noalias(*C.dense()) = *A.dense() + *B.banded();
break;
case 7:
noalias(*C.dense()) = *A.dense() + *B.identity();
break;
default:
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
}
else if (numA == 2)
switch (numB)
{
case 1:
noalias(*C.dense()) = *A.triang() + *B.dense();
break;
case 3:
noalias(*C.dense()) = *A.triang() + *B.sym();
break;
case 4:
noalias(*C.dense()) = *A.triang() + *B.sparse();
break;
case 5:
noalias(*C.dense()) = *A.triang() + *B.banded();
break;
case 7:
noalias(*C.dense()) = *A.triang() + *B.identity();
break;
default:
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
}
else if (numA == 3)
switch (numB)
{
case 1:
noalias(*C.dense()) = *A.sym() + *B.dense();
break;
case 2:
noalias(*C.dense()) = *A.sym() + *B.triang();
break;
case 4:
noalias(*C.dense()) = *A.sym() + *B.sparse();
break;
case 5:
noalias(*C.dense()) = *A.sym() + *B.banded();
break;
case 7:
noalias(*C.dense()) = *A.sym() + *B.identity();
break;
default:
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
}
else if (numA == 4)
switch (numB)
{
case 1:
noalias(*C.dense()) = *A.sparse() + *B.dense();
break;
case 2:
noalias(*C.dense()) = *A.sparse() + *B.triang();
break;
case 3:
noalias(*C.dense()) = *A.sparse() + *B.sym();
break;
case 5:
noalias(*C.dense()) = *A.sparse() + *B.banded();
break;
case 7:
noalias(*C.dense()) = *A.sparse() + *B.identity();
break;
default:
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
}
else if (numA == 5)
switch (numB)
{
case 1:
noalias(*C.dense()) = *A.banded() + *B.dense();
break;
case 2:
noalias(*C.dense()) = *A.banded() + *B.triang();
break;
case 3:
noalias(*C.dense()) = *A.banded() + *B.sym();
break;
case 4:
noalias(*C.dense()) = *A.banded() + *B.sparse();
break;
case 7:
noalias(*C.dense()) = *A.banded() + *B.identity();
break;
default:
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
}
else if (numA == 7)
switch (numB)
{
case 1:
noalias(*C.dense()) = *A.identity() + *B.dense();
break;
case 2:
noalias(*C.dense()) = *A.identity() + *B.triang();
break;
case 3:
noalias(*C.dense()) = *A.identity() + *B.sym();
break;
case 4:
noalias(*C.dense()) = *A.identity() + *B.sparse();
break;
case 5:
noalias(*C.dense()) = *A.identity() + *B.banded();
break;
default:
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
}
else
SiconosMatrixException::selfThrow("Matrix function add(A,B,C): invalid type of matrix");
C.resetLU();
}
else // A and/or B is Block
{
if (numA != 0) // A Simple, whatever is B
{
C = A;
C += B;
}
else // A Block
{
C = B;
C += A;
}
}
}
}
}
}
const SimpleMatrix operator - (const SiconosMatrix& A, const SiconosMatrix& B)
{
// To compute C = A - B
if ((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
SiconosMatrixException::selfThrow("Matrix operator - : inconsistent sizes");
unsigned int numA = A.getNum();
unsigned int numB = B.getNum();
// == B equal to null ==
if (numB == 6)
return SimpleMatrix(A);
// == B different from 0 ==
if (numA == numB && numA != 0) // all matrices are of the same type and NOT block
{
if (numA == 1)
return (DenseMat)(*A.dense() - *B.dense());
else if (numA == 2)
return (TriangMat)(*A.triang() - *B.triang());
else if (numA == 3)
return (SymMat)(*A.sym() - *B.sym());
else if (numA == 4)
{
SparseMat tmp(*A.sparse());
tmp -= *B.sparse();
return tmp;
//return (SparseMat)(*A.sparse() - *B.sparse());
}
else //if(numA==5)
{
BandedMat tmp(*A.banded());
tmp -= *B.banded();
return tmp;
//return (BandedMat)(*A.banded() - *B.banded());
}
}
else if (numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
{
if (numA == 1)
{
if (numB == 2)
return (DenseMat)(*A.dense() - *B.triang());
else if (numB == 3)
return (DenseMat)(*A.dense() - *B.sym());
else if (numB == 4)
return (DenseMat)(*A.dense() - *B.sparse());
else if (numB == 5)
return (DenseMat)(*A.dense() - *B.banded());
else // if(numB ==7)
return (DenseMat)(*A.dense() - *B.identity());
}
else if (numA == 2)
{
if (numB == 1)
return (DenseMat)(*A.triang() - *B.dense());
else if (numB == 3)
return (DenseMat)(*A.triang() - *B.sym());
else if (numB == 4)
return (DenseMat)(*A.triang() - *B.sparse());
else if (numB == 5)
return (DenseMat)(*A.triang() - *B.banded());
else // if(numB ==7:
return (DenseMat)(*A.triang() - *B.identity());
}
else if (numA == 3)
{
if (numB == 1)
return (DenseMat)(*A.sym() - *B.dense());
else if (numB == 2)
return (DenseMat)(*A.sym() - *B.triang());
else if (numB == 4)
return (DenseMat)(*A.sym() - *B.sparse());
else if (numB == 5)
return (DenseMat)(*A.sym() - *B.banded());
else // if(numB ==7)
return (DenseMat)(*A.sym() - *B.identity());
}
else if (numA == 4)
{
if (numB == 1)
return (DenseMat)(*A.sparse() - *B.dense());
else if (numB == 2)
return (DenseMat)(*A.sparse() - *B.triang());
else if (numB == 3)
return (DenseMat)(*A.sparse() - *B.sym());
else if (numB == 5)
return (DenseMat)(*A.sparse() - *B.banded());
else // if(numB ==7)
return (DenseMat)(*A.sparse() - *B.identity());
}
else if (numA == 5)
{
if (numB == 1)
return (DenseMat)(*A.banded() - *B.dense());
else if (numB == 2)
return (DenseMat)(*A.banded() - *B.triang());
else if (numB == 3)
return (DenseMat)(*A.banded() - *B.sym());
else if (numB == 4)
return (DenseMat)(*A.banded() - *B.sparse());
else //if(numB ==7)
return (DenseMat)(*A.banded() - *B.identity());
}
else if (numA == 6)
{
if (numB == 1)
return (DenseMat)(*A.zero_mat() - *B.dense());
else if (numB == 2)
return (DenseMat)(*A.zero_mat() - *B.triang());
else if (numB == 3)
return (DenseMat)(*A.zero_mat() - *B.sym());
else if (numB == 4)
return (DenseMat)(*A.zero_mat() - *B.sparse());
else //if(numB ==7)
return (DenseMat)(*A.zero_mat() - *B.identity());
}
else //if(numA==7)
{
if (numB == 1)
return (DenseMat)(*A.identity() - *B.dense());
else if (numB == 2)
return (DenseMat)(*A.identity() - *B.triang());
else if (numB == 3)
return (DenseMat)(*A.identity() - *B.sym());
else if (numB == 4)
return (DenseMat)(*A.identity() - *B.sparse());
else //if(numB ==5)
return (DenseMat)(*A.identity() - *B.banded());
}
}
else // A and/or B are/is Block
{
SimpleMatrix tmp(A);
tmp -= B;
return tmp;
}
}
const SimpleMatrix operator + (const SiconosMatrix& A, const SiconosMatrix& B)
{
// To compute C = A + B
if ((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
SiconosMatrixException::selfThrow("Matrix operator + : inconsistent sizes");
unsigned int numA = A.getNum();
unsigned int numB = B.getNum();
// == A or B equal to null ==
if (numA == 6) // A = 0
{
if (numB == 6) // B = 0
return SimpleMatrix(A.size(0), A.size(1));
else
return SimpleMatrix(B);
}
if (numB == 6)
return SimpleMatrix(A);
// == A and B different from 0 ==
if (numA == numB && numA != 0) // all matrices are of the same type and NOT block
{
if (numA == 1)
return (DenseMat)(*A.dense() + *B.dense());
else if (numA == 2)
return (TriangMat)(*A.triang() + *B.triang());
else if (numA == 3)
return (SymMat)(*A.sym() + *B.sym());
else if (numA == 4)
{
SparseMat tmp(*A.sparse());
tmp += *B.sparse();
return tmp;
// return (SparseMat)(*A.sparse() + *B.sparse());
}
else //if(numA==5)
{
BandedMat tmp(*A.banded());
tmp += *B.banded();
return tmp;
}
}
else if (numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
{
if (numA == 1)
{
if (numB == 2)
return (DenseMat)(*A.dense() + *B.triang());
else if (numB == 3)
return (DenseMat)(*A.dense() + *B.sym());
else if (numB == 4)
return (DenseMat)(*A.dense() + *B.sparse());
else if (numB == 5)
return (DenseMat)(*A.dense() + *B.banded());
else // if(numB ==7)
return (DenseMat)(*A.dense() + *B.identity());
}
else if (numA == 2)
{
if (numB == 1)
return (DenseMat)(*A.triang() + *B.dense());
else if (numB == 3)
return (DenseMat)(*A.triang() + *B.sym());
else if (numB == 4)
return (DenseMat)(*A.triang() + *B.sparse());
else if (numB == 5)
return (DenseMat)(*A.triang() + *B.banded());
else // if(numB ==7:
return (DenseMat)(*A.triang() + *B.identity());
}
else if (numA == 3)
{
if (numB == 1)
return (DenseMat)(*A.sym() + *B.dense());
else if (numB == 2)
return (DenseMat)(*A.sym() + *B.triang());
else if (numB == 4)
return (DenseMat)(*A.sym() + *B.sparse());
else if (numB == 5)
return (DenseMat)(*A.sym() + *B.banded());
else // if(numB ==7)
return (DenseMat)(*A.sym() + *B.identity());
}
else if (numA == 4)
{
if (numB == 1)
return (DenseMat)(*A.sparse() + *B.dense());
else if (numB == 2)
return (DenseMat)(*A.sparse() + *B.triang());
else if (numB == 3)
return (DenseMat)(*A.sparse() + *B.sym());
else if (numB == 5)
return (DenseMat)(*A.sparse() + *B.banded());
else // if(numB ==7)
return (DenseMat)(*A.sparse() + *B.identity());
}
else if (numA == 5)
{
if (numB == 1)
return (DenseMat)(*A.banded() + *B.dense());
else if (numB == 2)
return (DenseMat)(*A.banded() + *B.triang());
else if (numB == 3)
return (DenseMat)(*A.banded() + *B.sym());
else if (numB == 4)
return (DenseMat)(*A.banded() + *B.sparse());
else //if(numB ==7)
return (DenseMat)(*A.banded() + *B.identity());
}
else //if(numA==7)
{
if (numB == 1)
return (DenseMat)(*A.identity() + *B.dense());
else if (numB == 2)
return (DenseMat)(*A.identity() + *B.triang());
else if (numB == 3)
return (DenseMat)(*A.identity() + *B.sym());
else if (numB == 4)
return (DenseMat)(*A.identity() + *B.sparse());
else //if(numB ==5)
return (DenseMat)(*A.identity() + *B.banded());
}
}
else if (numB != 0) // B Simple, whatever is A
{
SimpleMatrix tmp(B);
tmp += A;
return tmp;
}
else // B Block, A simple or block
{
SimpleMatrix tmp(A);
tmp += B;
return tmp;
}
}
void SimpleMatrix::addBlock(unsigned int row_min, unsigned int col_min, const SiconosMatrix& m)
{
// add m to current matrix elements, starting from row row_min and column col_min, to the values of the matrix m.
// m may be a BlockMatrix.
if (_num == 6 || _num == 7)
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(pos,..., m) forbidden for zero or identity matrix.");
if (&m == this)
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(pos,..., m): m = this.");
if (row_min >= size(0))
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(row,col): row is out of range");
if (col_min >= size(1))
SiconosMatrixException::selfThrow("SimpleMatrix::addBloc(row,col)k: col is out of range");
unsigned int row_max, col_max;
row_max = m.size(0) + row_min;
col_max = m.size(1) + col_min;
if (row_max > size(0))
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(row,col,m): m.row + row is out of range.");
if (col_max > size(1))
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(row,col,m): m.col + col is out of range.");
unsigned int numM = m.num();
if (numM == 0) // if m is a block matrix ...
{
const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
BlocksMat::const_iterator1 it;
BlocksMat::const_iterator2 it2;
unsigned int posRow = row_min;
unsigned int posCol = col_min;
for (it = mB._mat->begin1(); it != mB._mat->end1(); ++it)
{
for (it2 = it.begin(); it2 != it.end(); ++it2)
{
addBlock(posRow, posCol, **it2);
posCol += (*it2)->size(1);
}
posRow += (*it)->size(0);
posCol = 0;
}
}
else if (numM == 6) // if m = 0
{
// nothing to do !
}
else // if m is a SimpleMatrix
{
if (_num == 1)
{
switch (numM)
{
case 1:
noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) += *(m.dense());
break;
case 2:
noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) += *(m.triang());
break;
case 3:
noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) += *(m.sym());
break;
case 4:
noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) += *(m.sparse());
break;
case 5:
noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) += *(m.banded());
break;
case 7:
noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) += *(m.identity());
break;
default:
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(...,m): wrong matrix type for m.");
break;
}
}
else
SiconosMatrixException::selfThrow("SimpleMatrix::addBlock(...): implemeted only for dense matrices.");
resetLU();
}
}