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."); } }