void FirstOrderType1R::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
// Check if an Interaction is connected to the Relation.
unsigned int sizeY = inter.getSizeOfY();
unsigned int sizeDS = inter.getSizeOfDS();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
workV.resize(FirstOrderR::workVecSize);
workV[FirstOrderR::vec_z].reset(new SiconosVector(sizeZ));
workV[FirstOrderR::vec_x].reset(new SiconosVector(sizeDS));
workV[FirstOrderR::vec_r].reset(new SiconosVector(sizeDS));
workM.resize(FirstOrderR::mat_workMatSize);
if (!_C)
workM[FirstOrderR::mat_C].reset(new SimpleMatrix(sizeY, sizeDS));
if (!_D)
workM[FirstOrderR::mat_D].reset(new SimpleMatrix(sizeY, sizeY));
if (!_F)
workM[FirstOrderR::mat_F].reset(new SimpleMatrix(sizeY, sizeZ));
if (!_B)
workM[FirstOrderR::mat_B].reset(new SimpleMatrix(sizeDS, sizeY));
}
void FirstOrderLinearR::initialize(Interaction& inter)
{
FirstOrderR::initialize(inter);
// get interesting size
unsigned int sizeY = inter.dimension();
unsigned int sizeX = inter.getSizeOfDS();
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
VectorOfSMatrices& relationMat = inter.relationMatrices();
VectorOfVectors & relationVec= inter.relationVectors();
if (!_C && _pluginJachx->fPtr)
relationMat[FirstOrderR::mat_C].reset(new SimpleMatrix(sizeY, sizeX));
if (!_D && _pluginJachlambda->fPtr)
relationMat[FirstOrderR::mat_D].reset(new SimpleMatrix(sizeY, sizeY));
if (!_B && _pluginJacglambda->fPtr)
relationMat[FirstOrderR::mat_B].reset(new SimpleMatrix(sizeX, sizeY));
if (!_F && _pluginf->fPtr)
relationMat[FirstOrderR::mat_F].reset(new SimpleMatrix(sizeY, sizeZ));
if (!_e && _plugine->fPtr)
relationVec[FirstOrderR::e].reset(new SiconosVector(sizeY));
checkSize(inter);
}
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom3DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
NewtonEulerFrom1DLocalFrameR::initComponents(inter, DSlink, workV, workM);
unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
/*keep only the distance.*/
_jachq.reset(new SimpleMatrix(3, qSize));
_Mabs_C.reset(new SimpleMatrix(3, 3));
_AUX2.reset(new SimpleMatrix(3, 3));
// _isContact=1;
}
void KneeJointR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
NewtonEulerR::initComponents(inter, DSlink, workV, workM);
if (!_dotjachq)
{
unsigned int sizeY = inter.getSizeOfY();
unsigned int xSize = inter.getSizeOfDS();
unsigned int qSize = 7 * (xSize / 6);
_dotjachq.reset(new SimpleMatrix(sizeY, qSize));
}
}
void FirstOrderLinearR::checkSize(Interaction& inter)
{
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
// get interesting size
unsigned int sizeY = inter.dimension();
unsigned int sizeX = inter.getSizeOfDS();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
// Check if various operators sizes are consistent.
// Reference: interaction.
if (_C)
{
if (_C->size(0) == 0)
_C->resize(sizeX, sizeY);
else
assert((_C->size(0) == sizeY && _C->size(1) == sizeX) && "FirstOrderLinearR::initialize , inconsistent size between C and Interaction.");
}
if (_B)
{
if (_B->size(0) == 0)
_B->resize(sizeY, sizeX);
else
assert((_B->size(1) == sizeY && _B->size(0) == sizeX) && "FirstOrderLinearR::initialize , inconsistent size between B and interaction.");
}
// C and B are the minimum inputs. The others may remain null.
if (_D)
{
if (_D->size(0) == 0)
_D->resize(sizeY, sizeY);
else
assert((_D->size(0) == sizeY || _D->size(1) == sizeY) && "FirstOrderLinearR::initialize , inconsistent size between C and D.");
}
if (_F)
{
if (_F->size(0) == 0)
_F->resize(sizeY, sizeZ);
else
assert(((_F->size(0) == sizeY) && (_F->size(1) == sizeZ)) && "FirstOrderLinearR::initialize , inconsistent size between C and F.");
}
if (_e)
{
if (_e->size() == 0)
_e->resize(sizeY);
else
assert(_e->size() == sizeY && "FirstOrderLinearR::initialize , inconsistent size between C and e.");
}
}
void NewtonEulerR::initialize(Interaction& inter)
{
DEBUG_BEGIN("NewtonEulerR::initialize(Interaction& inter)\n");
unsigned int ySize = inter.dimension();
unsigned int xSize = inter.getSizeOfDS();
unsigned int qSize = 7 * (xSize / 6);
if (!_jachq)
_jachq.reset(new SimpleMatrix(ySize, qSize));
else
{
if (_jachq->size(0) == 0)
{
// if the matrix dim are null
_jachq->resize(ySize, qSize);
}
else
{
assert((_jachq->size(1) == qSize && _jachq->size(0) == ySize) ||
(printf("NewtonEuler::initializeWorkVectorsAndMatrices _jachq->size(1) = %d ,_qsize = %d , _jachq->size(0) = %d ,_ysize =%d \n", _jachq->size(1), qSize, _jachq->size(0), ySize) && false) ||
("NewtonEuler::initializeWorkVectorsAndMatrices inconsistent sizes between _jachq matrix and the interaction." && false));
}
}
DEBUG_EXPR(_jachq->display());
if (! _jachqT)
_jachqT.reset(new SimpleMatrix(ySize, xSize));
if (! _T)
{
_T.reset(new SimpleMatrix(7, 6));
_T->zero();
_T->setValue(0, 0, 1.0);
_T->setValue(1, 1, 1.0);
_T->setValue(2, 2, 1.0);
}
DEBUG_EXPR(_jachqT->display());
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
if (!_contactForce)
{
_contactForce.reset(new SiconosVector(DSlink[NewtonEulerR::p1]->size()));
_contactForce->zero();
}
DEBUG_END("NewtonEulerR::initialize(Interaction& inter)\n");
}
void NewtonEulerFrom1DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
NewtonEulerR::initComponents(inter, DSlink, workV, workM);
//proj_with_q _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));
unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
_jachq.reset(new SimpleMatrix(1, qSize));
/* VA 12/04/2016 All of what follows should be put in WorkM*/
_RotationAbsToContactFrame.reset(new SimpleMatrix(1, 3));
_rotationMatrixAbsToBody.reset(new SimpleMatrix(3, 3));
_AUX1.reset(new SimpleMatrix(3, 3));
_AUX2.reset(new SimpleMatrix(1, 3));
_NPG1.reset(new SimpleMatrix(3, 3));
_NPG2.reset(new SimpleMatrix(3, 3));
// _isContact=1;
}
void LagrangianLinearTIR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
unsigned int sizeY = inter.getSizeOfY();
if (!(_jachq) || _jachq->size(1) != inter.getSizeOfDS() || _jachq->size(0) != sizeY)
RuntimeException::selfThrow("LagrangianLinearTIR::initComponents inconsistent sizes between H matrix and the interaction.");
if ((_jachlambda) && (_jachlambda->size(0) != sizeY || _jachlambda->size(1) != sizeY))
RuntimeException::selfThrow("LagrangianLinearTIR::initComponents inconsistent sizes between D matrix and the interaction.");
if ((_e) && _e->size() != sizeY)
RuntimeException::selfThrow("LagrangianLinearTIR::initComponents inconsistent sizes between e vector and the dimension of the interaction.");
unsigned int sizeZ = DSlink[LagrangianR::z]->size();
if ((_F) && (
_F->size(0) != sizeZ || _F->size(1) != sizeZ))
RuntimeException::selfThrow("LagrangianLinearTIR::initComponents inconsistent sizes between F matrix and the interaction.");
}
void NewtonEulerR::checkSize(Interaction& inter)
{
assert((_jachq->size(1) == 7 * (inter.getSizeOfDS() / 6) && _jachq->size(0) == inter.dimension()) ||
(printf("NewtonEuler::initializeWorkVectorsAndMatrices _jachq->size(1) = %d ,_qsize = %d , _jachq->size(0) = %d ,_ysize =%d \n", _jachq->size(1), 7 * (inter.getSizeOfDS() / 6), _jachq->size(0), inter.dimension()) && false) ||
("NewtonEuler::initializeWorkVectorsAndMatrices inconsistent sizes between _jachq matrix and the interaction." && false));
}
void FirstOrderLinearR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
// Note: do not call FirstOrderR::initialize to avoid jacobianH and jacobianG allocation.
// get interesting size
unsigned int sizeY = inter.getSizeOfY();
unsigned int sizeX = inter.getSizeOfDS();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
// Update workV (copy of DS variables)
workV.resize(FirstOrderR::workVecSize);
workV[FirstOrderR::vec_z].reset(new SiconosVector(sizeZ));
workM.resize(FirstOrderR::mat_workMatSize);
if (!_C && _pluginJachx->fPtr)
workM[FirstOrderR::mat_C].reset(new SimpleMatrix(sizeY, sizeX));
if (!_D && _pluginJachlambda->fPtr)
workM[FirstOrderR::mat_D].reset(new SimpleMatrix(sizeY, sizeY));
if (!_B && _pluginJacglambda->fPtr)
workM[FirstOrderR::mat_B].reset(new SimpleMatrix(sizeX, sizeY));
if (!_F && _pluginf->fPtr)
{
workM[FirstOrderR::mat_F].reset(new SimpleMatrix(sizeY, sizeZ));
}
if (!_e && _plugine->fPtr)
{
workV[FirstOrderR::e].reset(new SiconosVector(sizeY));
}
// Check if various operators sizes are consistent.
// Reference: interaction.
if (_C)
{
if (_C->size(0) == 0)
_C->resize(sizeX, sizeY);
else
assert((_C->size(0) == sizeY && _C->size(1) == sizeX) && "FirstOrderLinearR::initialize , inconsistent size between C and Interaction.");
}
if (_B)
{
if (_B->size(0) == 0)
_B->resize(sizeY, sizeX);
else
assert((_B->size(1) == sizeY && _B->size(0) == sizeX) && "FirstOrderLinearR::initialize , inconsistent size between B and interaction.");
}
// C and B are the minimum inputs. The others may remain null.
if (_D)
{
if (_D->size(0) == 0)
_D->resize(sizeY, sizeY);
else
assert((_D->size(0) == sizeY || _D->size(1) == sizeY) && "FirstOrderLinearR::initialize , inconsistent size between C and D.");
}
if (_F)
{
if (_F->size(0) == 0)
_F->resize(sizeY, sizeZ);
else
assert(((_F->size(0) == sizeY) && (_F->size(1) == sizeZ)) && "FirstOrderLinearR::initialize , inconsistent size between C and F.");
}
if (_e)
{
if (_e->size() == 0)
_e->resize(sizeY);
else
assert(_e->size() == sizeY && "FirstOrderLinearR::initialize , inconsistent size between C and e.");
}
}
