void FirstOrderType1R::computeh(double time, SiconosVector& x, SiconosVector& z, SiconosVector& y)
{
assert(_pluginh && "FirstOrderType1R::computeOutput() is not linked to a plugin function");
((Type1Ptr)(_pluginh->fPtr))(x.size(), &(x)(0), y.size(), &(y)(0), z.size(), &(z)(0));
}
void FirstOrderType1R::computeg(double time, SiconosVector& lambda, SiconosVector& z, SiconosVector& r)
{
assert(_pluging && "FirstOrderType1R::computeInput() is not linked to a plugin function");
((Type1Ptr)(_pluging->fPtr))(lambda.size(), &(lambda)(0), r.size(), &(r)(0), z.size(), &(z)(0));
}
void SiconosVector::toBlock(SiconosVector& vOut, unsigned int sizeB, unsigned int startIn, unsigned int startOut) const
{
// To copy a subBlock of the vector (from position startIn to startIn+sizeB) into vOut (from pos. startOut to startOut+sizeB).
// Check dim ...
assert(startIn < size() && "vector toBlock(v1,v2,...): start position in input vector is out of range.");
assert(startOut < vOut.size() && "vector toBlock(v1,v2,...): start position in output vector is out of range.");
assert(startIn + sizeB <= size() && "vector toBlock(v1,v2,...): end position in input vector is out of range.");
assert(startOut + sizeB <= vOut.size() && "vector toBlock(v1,v2,...): end position in output vector is out of range.");
unsigned int endOut = startOut + sizeB;
unsigned int numIn = num();
unsigned int numOut = vOut.num();
if (numIn == numOut)
{
if (numIn == 1) // vIn / vOut are Dense
noalias(ublas::subrange(*vOut.dense(), startOut, endOut)) = ublas::subrange(*vect.Dense, startIn, startIn + sizeB);
else // if(numIn == 4)// vIn / vOut are Sparse
noalias(ublas::subrange(*vOut.sparse(), startOut, endOut)) = ublas::subrange(*vect.Sparse, startIn, startIn + sizeB);
}
else // vIn and vout of different types ...
{
if (numIn == 1) // vIn Dense
noalias(ublas::subrange(*vOut.sparse(), startOut, endOut)) = ublas::subrange(*vect.Dense, startIn, startIn + sizeB);
else // if(numIn == 4)// vIn Sparse
noalias(ublas::subrange(*vOut.dense(), startOut, endOut)) = ublas::subrange(*vect.Sparse, startIn, startIn + sizeB);
}
}
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));
}
void FirstOrderLinearR::computee(double time, SiconosVector& z, SiconosVector& e)
{
if (_plugine->fPtr)
{
((FOVecPtr) _plugine->fPtr)(time, e.size(), &(e)(0), z.size(), &(z)(0));
}
}
void LagrangianCompliantR::computeJachlambda(double time, SiconosVector& q0, SiconosVector& lambda, SiconosVector& z)
{
if (_pluginJachlambda->fPtr)
{
// get vector lambda of the current interaction
((FPtr2)_pluginJachlambda->fPtr)(q0.size(), &(q0)(0), lambda.size(), &(lambda)(0), &(*_jachlambda)(0, 0), z.size(), &(z)(0));
// Copy data that might have been changed in the plug-in call.
}
}
void LagrangianCompliantR::computeh(double time, SiconosVector& q0, SiconosVector& lambda, SiconosVector& z, SiconosVector& y)
{
if (_pluginh->fPtr)
{
// get vector y of the current interaction
// Warning: temporary method to have contiguous values in memory, copy of block to simple.
((FPtr2)(_pluginh->fPtr))(q0.size(), &(q0)(0), y.size(), &(lambda)(0), &(y)(0), z.size(), &(z)(0));
}
}
void LagrangianCompliantR::computeJachq(double time, SiconosVector& q0, SiconosVector& lambda, SiconosVector& z)
{
if (_pluginJachq->fPtr)
{
// Warning: temporary method to have contiguous values in memory, copy of block to simple.
// get vector lambda of the current interaction
((FPtr2)(_pluginJachq->fPtr))(q0.size(), &(q0)(0), lambda.size(), &(lambda)(0), &(*_jachq)(0, 0), z.size(), &(z)(0));
// Copy data that might have been changed in the plug-in call.
}
}
void LagrangianRheonomousR::computeh(double time, SiconosVector& q, SiconosVector& z, SiconosVector& y)
{
DEBUG_PRINT(" LagrangianRheonomousR::computeh(double time,Interaction& inter, SP::BlockVector q, SP::BlockVector z)");
if (_pluginh)
{
// arg= time. Unused in this function but required for interface.
if (_pluginh->fPtr)
{
((FPtr4)(_pluginh->fPtr))(q.size(), &(q)(0), time, y.size(), &(y)(0), z.size(), &(z)(0));
}
}
}
void LagrangianRheonomousR::computehDot(double time, SiconosVector& q, SiconosVector& z)
{
if (_pluginhDot && _pluginhDot->fPtr)
{
((FPtr4)(_pluginhDot->fPtr))(q.size(), &(q)(0), time, _hDot->size(), &(*_hDot)(0), z.size(), &(z)(0));
}
}
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 SiconosVector::setVector(unsigned int , const SiconosVector& newV)
{
if (newV.size() != size())
SiconosVectorException::selfThrow("SiconosVector::setVector(num,v), unconsistent sizes.");
*this = newV ;
}
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));
}
}
void ControlLinearAdditionalTermsED::addSmoothTerms(DynamicalSystemsGraph& DSG0, const DynamicalSystemsGraph::VDescriptor& dsgVD, const double t, SiconosVector& xdot)
{
// check whether we have a system with a control input
if (DSG0.u.hasKey(dsgVD))
{
if (DSG0.B.hasKey(dsgVD))
{
prod(DSG0.B.getRef(dsgVD), DSG0.u.getRef(dsgVD), xdot, false); // xdot += B*u
}
else if (DSG0.pluginU.hasKey(dsgVD))
{
DynamicalSystem& ds = *DSG0.bundle(dsgVD);
SiconosVector& u = DSG0.u.getRef(dsgVD);
SiconosVector& tmpXdot = DSG0.tmpXdot.getRef(dsgVD);
((AdditionalTermsEDfctU)DSG0.pluginU.getRef(dsgVD).fPtr)(t, xdot.size(), ds.getx().getArray(), u.size(), u.getArray(), tmpXdot.getArray(), ds.getz().size(), ds.getz().getArray());
xdot += tmpXdot; // xdot += g(x, u)
}
else
{
RuntimeException::selfThrow("ControlLinearAdditionalTermsED :: input u but no B nor pluginU");
}
}
// check whether the DynamicalSystem is an Observer
if (DSG0.e.hasKey(dsgVD))
{
assert(DSG0.L.hasKey(dsgVD));
prod(*DSG0.L[dsgVD], *DSG0.e[dsgVD], xdot, false); // xdot += -L*e
}
}
void SimpleMatrix::SolveByLeastSquares(SiconosVector &B)
{
if (B.isBlock())
SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares(SiconosVector &B) failed. Not yet implemented for V being a BlockVector.");
DenseMat tmpB(B.size(), 1);
ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
int info = 0;
#ifdef USE_OPTIMAL_WORKSPACE
info += lapack::gels(*mat.Dense, tmpB, lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
info += lapack::gels(*mat.Dense, tmpB, lapack::minimal_workspace());
#endif
if (info != 0)
{
std::cout << "info = " << info << std::endl;
SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares failed.");
}
else
{
noalias(*(B.dense())) = ublas::column(tmpB, 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 SimpleMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
if (B.isBlock())
SiconosMatrixException::selfThrow("SimpleMatrix PLUForwardBackwardInPlace(V) failed. Not yet implemented for V being a BlockVector.");
DenseMat tmpB(B.size(), 1);
ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
int info;
if (_num == 1)
{
if (!_isPLUFactorized) // call gesv => LU-factorize+solve
{
// solve system:
if (!_ipiv)
_ipiv.reset(new VInt(size(0)));
else
_ipiv->resize(size(0));
info = lapack::gesv(*mat.Dense, *_ipiv, tmpB);
_isPLUFactorized = true;
/*
ublas::matrix<double> COPY(*mat.Dense);
ublas::vector<double> S(std::max(size(0),size(1)));
ublas::matrix<double, ublas::column_major> U(size(0),size(1));
ublas::matrix<double, ublas::column_major> VT(size(0),size(1));
int ierr = lapack::gesdd(COPY, S, U, VT);
printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
*/
// B now contains solution:
}
else // call getrs: only solve using previous lu-factorization
info = lapack::getrs(*mat.Dense, *_ipiv, tmpB);
}
else
{
if (!_isPLUFactorized) // call first PLUFactorizationInPlace
{
PLUFactorizationInPlace();
}
// and then solve
inplace_solve(*sparse(), tmpB, ublas::lower_tag());
inplace_solve(ublas::trans(*sparse()), tmpB, ublas::upper_tag());
info = 0;
}
if (info != 0)
SiconosMatrixException::selfThrow("SimpleMatrix::PLUForwardBackwardInPlace failed.");
else
{
noalias(*(B.dense())) = ublas::column(tmpB, 0);
}
}
SiconosVector::SiconosVector(const SiconosVector& v1, const SiconosVector& v2)
{
unsigned int size1 = v1.size();
if (ask<IsDense>(v1) && ask<IsDense>(v2))
{
_dense = true;
vect.Dense = new DenseVect(size1 + v2.size());
}
else if (ask<IsSparse>(v1) && ask<IsSparse>(v2))
{
_dense = false;
vect.Sparse = new SparseVect(size1 + v2.size());
}
else
{
SiconosVectorException::selfThrow("SiconosVector::SiconosVector :: mixed dense and sparse vector detected");
}
setBlock(0, v1);
setBlock(size1, v2);
}
void DynamicalSystem::setR(const SiconosVector& newValue)
{
// check dimensions ...
if (newValue.size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setR - inconsistent sizes between x0 input and n - Maybe you forget to set n?");
if (_r)
*_r = newValue;
else
_r.reset(new SiconosVector(newValue));
}
void LagrangianRheonomousR::computeJachq(double time, SiconosVector& q, SiconosVector& z)
{
// Note that second input arg is useless.
if (_pluginJachq->fPtr)
{
// Warning: temporary method to have contiguous values in
// memory, copy of block to simple.
((FPtr4)(_pluginJachq->fPtr))(q.size(), &(q)(0), time, _jachq->size(0), &(*_jachq)(0, 0), z.size(), &(z)(0));
// Copy data that might have been changed in the plug-in call.
}
// else nothing.
}
void FirstOrderLinearR::computeg(double time, SiconosVector& lambda, SiconosVector& z, BlockVector& r)
{
if (_pluginJacglambda->fPtr)
{
if (!_B)
_B.reset(new SimpleMatrix(r.size(),lambda.size()));
computeB(time, z, *_B);
}
prod(*_B, lambda, r, false);
}
// Copy
SiconosVector::SiconosVector(const SiconosVector &svect) : std11::enable_shared_from_this<SiconosVector>()
{
if (ask<IsDense>(svect)) // dense
{
_dense = true;
vect.Dense = new DenseVect(svect.size());
noalias(*vect.Dense) = (*svect.dense());
// std::copy((vect.Dense)->begin(), (vect.Dense)->end(), (svect.dense())->begin());
}
else //sparse
{
_dense = false;
vect.Sparse = new SparseVect(svect.size());
noalias(*vect.Sparse) = (*svect.sparse());
//std::copy((vect.Sparse)->begin(), (vect.Sparse)->end(), (svect.sparse())->begin());
}
// Note FP: using constructor + noalias = (or std::copy) is more
// efficient than a call to ublas::vector copy constructor, this for
// large or small vectors.
}
void private_addprod(const SiconosMatrix& A, unsigned startRow, unsigned int startCol, const SiconosVector& x, SiconosVector& y)
{
assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");
assert(!A.isBlock() && "private_addprod(A,start,x,y) error: not yet implemented for block matrix.");
// we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
// Then computation of y = subA*x + y.
unsigned int numA = A.getNum();
unsigned int numY = y.getNum();
unsigned int numX = x.getNum();
unsigned int sizeX = x.size();
unsigned int sizeY = y.size();
assert(numX == numY && "private_addprod(A,start,x,y) error: not yet implemented for x and y of different types.");
if (numY == 1 && numX == 1)
{
assert(y.dense() != x.dense());
if (numA == 1)
noalias(*y.dense()) += prod(ublas::subrange(*A.dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
else if (numA == 2)
noalias(*y.dense()) += prod(ublas::subrange(*A.triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
else if (numA == 3)
noalias(*y.dense()) += prod(ublas::subrange(*A.sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
else if (numA == 4)
noalias(*y.dense()) += prod(ublas::subrange(*A.sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
else //if(numA==5)
noalias(*y.dense()) += prod(ublas::subrange(*A.banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.dense());
}
else // x and y sparse
{
if (numA == 4)
*y.sparse() += prod(ublas::subrange(*A.sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x.sparse());
else
SiconosMatrixException::selfThrow("private_addprod(A,start,x,y) error: not yet implemented for x, y sparse and A not sparse.");
}
}
void DynamicalSystem::setRhs(const SiconosVector& newValue)
{
// Warning: this only sets the value of x[1]
// check dimensions ...
if (newValue.size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setRhs - inconsistent sizes between x input and n - Maybe you forget to set n?");
if (! _x[1])
_x[1].reset(new SiconosVector(newValue));
else
*(_x[1]) = newValue;
}
void DynamicalSystem::setz(const SiconosVector& newValue)
{
if (_z)
{
if (newValue.size() != _z->size())
RuntimeException::selfThrow("DynamicalSystem::setz - inconsistent sizes between input and existing z - To change z size use setzPtr.");
*_z = newValue;
}
else
{
_z.reset(new SiconosVector(newValue));
}
}
void DynamicalSystem::setX(const SiconosVector& newValue)
{
// Warning: this only sets the value of x[0]
// We suppose that both x and (*x)[0] are properly allocated.
// check dimensions ...
if (newValue.size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setX - inconsistent sizes between x input and n - Maybe you forget to set n?");
if (! _x[0])
_x[0].reset(new SiconosVector(newValue));
else
*(_x[0]) = newValue;
}
void DynamicalSystem::setX0(const SiconosVector& newValue)
{
// check dimensions ...
if (newValue.size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setX0 - inconsistent sizes between x0 input and n - Maybe you forget to set n?");
if (_x0)
*_x0 = newValue;
else
{
_x0.reset(new SiconosVector(newValue));
}
_normRef = _x0->norm2() + 1;
}
void SiconosVector::subBlock(unsigned int index, const SiconosVector& vIn)
{
// Add vIn from the current vector, starting from position "index".
// vIn may be a BlockVector.
// if ( num != 1 ) SiconosVectorException::selfThrow("SiconosVector::subBlock : vector should be dense");
unsigned int end = vIn.size();
if ((index + end) > size()) SiconosVectorException::selfThrow("SiconosVector::subBlock : invalid ranges");
unsigned int numVin = vIn.num();
if (numVin != num()) SiconosVectorException::selfThrow("SiconosVector::subBlock : inconsistent types.");
if (_dense)
noalias(ublas::subrange(*vect.Dense, index, index + end)) -= *vIn.dense();
else
noalias(ublas::subrange(*vect.Sparse, index, index + end)) -= *vIn.sparse();
}
void SiconosVector::setBlock(unsigned int index, const SiconosVector& vIn)
{
// Set current vector elements, starting from position "index", to the values of vector vIn
// Exceptions ...
assert(&vIn != this && "SiconosVector::this->setBlock(pos,vIn): vIn = this.");
assert(index < size() && "SiconosVector::setBlock : invalid ranges");
unsigned int end = vIn.size() + index;
assert(end <= size() && "SiconosVector::setBlock : invalid ranges");
assert (vIn.num() == num() && "SiconosVector::setBlock: inconsistent types.");
if (_dense)
noalias(ublas::subrange(*vect.Dense, index, end)) = *vIn.dense();
else
noalias(ublas::subrange(*vect.Sparse, index, end)) = *vIn.sparse();
}