void expm(SiconosMatrix& A, SiconosMatrix& Exp, bool computeAndAdd)
{
// Implemented only for dense matrices.
// Note FP : Maybe it works for others but it has not been
// tested here --> to be done
// Do not work with sparse.
A.resetLU();
Exp.resetLU();
assert(Exp.num() == 1 || A.num() == 1);
if(computeAndAdd)
*Exp.dense() += expm_pad(*A.dense());
else
*Exp.dense() = expm_pad(*A.dense());
}
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;
}
}
}
}
}
}