void FirstOrderLinearR::computeB(double time, SiconosVector& z, SimpleMatrix& B)
{
if (_pluginJacglambda->fPtr)
{
((FOMatPtr1) _pluginJacglambda->fPtr)(time, B.size(0), B.size(1), &(B)(0, 0), z.size(), &(z)(0));
}
}
void FirstOrderLinearR::computeF(double time, SiconosVector& z, SimpleMatrix& F)
{
if (_pluginf->fPtr)
{
((FOMatPtr1)(_pluginf->fPtr))(time, F.size(0), F.size(1), &(F)(0, 0), z.size(), &(z)(0));
}
}
void FirstOrderLinearR::computeD(double time, SiconosVector& z, SimpleMatrix& D)
{
if (_pluginJachlambda->fPtr)
{
((FOMatPtr1)(_pluginJachlambda->fPtr))(time, D.size(0), D.size(1), &(D)(0, 0), z.size(), &(z)(0));
}
}
void FirstOrderLinearR::computeC(double time, SiconosVector& z, SimpleMatrix& C)
{
if (_pluginJachx->fPtr)
{
((FOMatPtr1)(_pluginJachx->fPtr))(time, C.size(0), C.size(1), &(C)(0, 0), z.size(), &(z)(0));
}
}
/* XXX Find out if we can use an elementwise ublas operation */
SP::SiconosVector compareMatrices(const SimpleMatrix& data, const SimpleMatrix& ref)
{
SimpleMatrix diff(data.size(0), data.size(1));
SP::SiconosVector res(new SiconosVector(data.size(1)));
diff = data - ref;
for (unsigned int i = 0; i < data.size(0); ++i)
{
for (unsigned int j = 0; j < data.size(1); ++j)
diff(i, j) /= 1 + fabs(ref(i, j));
}
diff.normInfByColumn(res);
return res;
}
void FirstOrderType1R::computeJachx(double time, SiconosVector& x, SiconosVector& z, SimpleMatrix& C)
{
//
assert(_pluginJachx && "FirstOrderType1R::computeJacobianH() failed; not linked to a plug-in function.");
((Type1Ptr)(_pluginJachx->fPtr))(x.size(), &(x)(0), C.size(0), C.getArray(), z.size(), &(z)(0));
}
// Copy constructors
SimpleMatrix::SimpleMatrix(const SimpleMatrix &smat): SiconosMatrix(smat.getNum()), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false)
{
if (num == 1)
{
mat.Dense = new DenseMat(smat.size(0), smat.size(1));
noalias(*mat.Dense) = (*smat.dense());
}
// mat.Dense = new DenseMat(*smat.dense());
else if (num == 2)
mat.Triang = new TriangMat(*smat.triang());
else if (num == 3)
mat.Sym = new SymMat(*smat.sym());
else if (num == 4)
mat.Sparse = new SparseMat(*smat.sparse());
else if (num == 5)
mat.Banded = new BandedMat(*smat.banded());
else if (num == 6)
mat.Zero = new ZeroMat(smat.size(0), smat.size(1));
else// if(num == 7)
mat.Identity = new IdentityMat(smat.size(0), smat.size(1));
}
void FirstOrderType1R::computeJacglambda(double time, SiconosVector& lambda, SiconosVector& z, SimpleMatrix& B)
{
assert(_pluginJacglambda && "FirstOrderType1R::computeJacobiang() failed; not linked to a plug-in function.");
((Type1Ptr)(_pluginJacglambda->fPtr))(lambda.size(), &(lambda)(0), B.size(0), B.getArray(), z.size(), &(z)(0));
}
void FirstOrderType1R::computeJachz(double time, SiconosVector& x, SiconosVector& z, SimpleMatrix& F)
{
if (_pluginJachz && _pluginJachz->fPtr)
((Type1Ptr)(_pluginJachz->fPtr))(x.size(), &(x)(0), F.size(0), F.getArray(), z.size(), &(z)(0));
}
const SimpleMatrix matrix_pow(const SimpleMatrix& m, unsigned int power)
{
if (m.isBlock())
SiconosMatrixException::selfThrow("Matrix, pow function: not yet implemented for BlockMatrix.");
if ( m.size(0) != m.size(1))
SiconosMatrixException::selfThrow("matrix_pow(SimpleMatrix), matrix is not square.");
if (power > 0)
{
unsigned int num = m.num();
if (num == 1)
{
DenseMat p = *m.dense();
for (unsigned int i = 1; i < power; i++)
p = prod(p, *m.dense());
return p;
}
else if (num == 2)
{
TriangMat t = *m.triang();
for (unsigned int i = 1; i < power; i++)
t = prod(t, *m.triang());
return t;
}
else if (num == 3)
{
SymMat s = *m.sym();
for (unsigned int i = 1; i < power; i++)
s = prod(s, *m.sym());
return s;
}
else if (num == 4)
{
SparseMat sp = *m.sparse();
for (unsigned int i = 1; i < power; i++)
sp = prod(sp, *m.sparse());
return sp;
}
else if (num == 5)
{
DenseMat b = *m.banded();
for (unsigned int i = 1; i < power; i++)
b = prod(b, *m.banded());
return b;
}
else if (num == 6)
{
ZeroMat z(m.size(0), m.size(1));
return z;
}
else // if (num==7)
{
IdentityMat I(m.size(0), m.size(1));;
return I;
}
}
else// if(power == 0)
{
IdentityMat I = ublas::identity_matrix<double>(m.size(0), m.size(1));
return I;
}
}
