SphereLDS::SphereLDS(double r, double m,
SP::SiconosVector qinit,
SP::SiconosVector vinit)
: LagrangianDS(qinit, vinit), radius(r), massValue(m)
{
normalize(q(), 3);
normalize(q(), 4);
normalize(q(), 5);
_ndof = 6;
assert(qinit->size() == _ndof);
assert(vinit->size() == _ndof);
_mass.reset(new SimpleMatrix(_ndof, _ndof));
_mass->zero();
I = massValue * radius * radius * 2. / 5.;
(*_mass)(0, 0) = (*_mass)(1, 1) = (*_mass)(2, 2) = massValue; ;
(*_mass)(3, 3) = (*_mass)(4, 4) = (*_mass)(5, 5) = I;
computeMass();
_jacobianFGyrq.reset(new SimpleMatrix(_ndof, _ndof));
_jacobianFGyrqDot.reset(new SimpleMatrix(_ndof, _ndof));
_fGyr.reset(new SiconosVector(_ndof));
_fGyr->zero();
computeFGyr();
}
void DynamicalSystem::setX0Ptr(SP::SiconosVector newPtr)
{
// check dimensions ...
if (newPtr->size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setX0Ptr - inconsistent sizes between x0 input and n - Maybe you forget to set n?");
_x0 = newPtr;
_normRef = _x0->norm2() + 1;
}
void DynamicalSystem::setRhsPtr(SP::SiconosVector newPtr)
{
// Warning: this only sets the pointer (*x)[1]
// check dimensions ...
if (newPtr->size() != _n)
RuntimeException::selfThrow("DynamicalSystem::setRhsPtr - inconsistent sizes between x input and n - Maybe you forget to set n?");
_x[1] = newPtr;
}
void SphereLDS::computeFGyr(SP::SiconosVector q, SP::SiconosVector v)
{
assert(q->size() == 6);
assert(v->size() == 6);
// normalize(q,3);
//normalize(q,4);
//normalize(q,5);
double theta = q->getValue(3);
double thetadot = v->getValue(3);
double phidot = v->getValue(4);
double psidot = v->getValue(5);
double sintheta = sin(theta);
(*_fGyr)(0) = (*_fGyr)(1) = (*_fGyr)(2) = 0;
(*_fGyr)(3) = I * psidot * phidot * sintheta;
(*_fGyr)(4) = -I * psidot * thetadot * sintheta;
(*_fGyr)(5) = -I * phidot * thetadot * sintheta;
}
void SimpleMatrix::normInfByColumn(SP::SiconosVector vIn) const
{
if (_num == 1)
{
if (vIn->size() != size(1))
RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: the given vector does not have the right length");
DenseVect tmpV = DenseVect(size(0));
for (unsigned int i = 0; i < size(1); i++)
{
ublas::noalias(tmpV) = ublas::column(*mat.Dense, i);
(*vIn)(i) = norm_inf(tmpV);
}
}
else
RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: not implemented for data other than DenseMat");
}
void private_addprod(double a, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SPC::SiconosVector x, SP::SiconosVector y)
{
assert(!(A->isPLUFactorized()) && "A is PLUFactorized in prod !!");
if (A->isBlock())
SiconosMatrixException::selfThrow("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();
if (numX != numY)
SiconosMatrixException::selfThrow("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()) += a * prod(ublas::subrange(*A->dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else if (numA == 2)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else if (numA == 3)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else if (numA == 4)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
else //if(numA==5)
noalias(*y->dense()) += a * prod(ublas::subrange(*A->banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
}
else // x and y sparse
{
if (numA == 4)
*y->sparse() += a * 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.");
}
}
