void MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds)
{
DEBUG_BEGIN("MoreauJeanDirectProjectionOSI::initializeWorkVectorsForDS( double t, SP::DynamicalSystem ds) \n");
MoreauJeanOSI::initializeWorkVectorsForDS(t, ds);
const DynamicalSystemsGraph::VDescriptor& dsv = _dynamicalSystemsGraph->descriptor(ds);
VectorOfVectors& workVectors = *_dynamicalSystemsGraph->properties(dsv).workVectors;
Type::Siconos dsType = Type::value(*ds);
if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->dimension()));
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS>(ds);
workVectors[MoreauJeanOSI::QTMP].reset(new SiconosVector(d->getqDim()));
}
else
{
RuntimeException::selfThrow("MoreauJeanDirectProjectionOSI::initialize() - DS not of the right type");
}
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
DEBUG_PRINTF("ds->initializeNonSmoothInput(%i)\n", k);
ds->initializeNonSmoothInput(k);
DEBUG_EXPR_WE(
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->p(k))
std::cout << "d->p(" << k <<" ) exists" << std::endl;
);
}
KneeJointR::KneeJointR(SP::NewtonEulerDS d1, SP::NewtonEulerDS d2, SP::SiconosVector P): NewtonEulerR()
{
_P0.reset(new SiconosVector(3));
*_P0 = *P;
/** Computation of _G1P0 and _G2P0 */
SP::SiconosVector q1 = d1->q0();
_G1P0x = _P0->getValue(0);
_G1P0y = _P0->getValue(1);
_G1P0z = _P0->getValue(2);
::boost::math::quaternion<double> quat1(q1->getValue(3), q1->getValue(4), q1->getValue(5), q1->getValue(6));
::boost::math::quaternion<double> quatG1P0(0, _G1P0x, _G1P0y, _G1P0z);
::boost::math::quaternion<double> quatBuff(0, 0, 0, 0);
quatBuff = quat1 * quatG1P0 / quat1;
SiconosVector P0_abs(3);
P0_abs.setValue(0, quatBuff.R_component_2() + q1->getValue(0));
P0_abs.setValue(1, quatBuff.R_component_3() + q1->getValue(1));
P0_abs.setValue(2, quatBuff.R_component_4() + q1->getValue(2));
// std::cout << "KneeJoint: P0_abs in the initial position.\n";
// P0_abs.display();
SP::SiconosVector q2 = d2->q0();
SiconosVector G2_abs(3);
G2_abs.setValue(0, q2->getValue(0));
G2_abs.setValue(1, q2->getValue(1));
G2_abs.setValue(2, q2->getValue(2));
::boost::math::quaternion<double> quat2_inv(q2->getValue(3), -q2->getValue(4), -q2->getValue(5), -q2->getValue(6));
SiconosVector G2P0_abs(3);
G2P0_abs = P0_abs - G2_abs;
::boost::math::quaternion<double> quatG2P0_abs(0, G2P0_abs.getValue(0), G2P0_abs.getValue(1), G2P0_abs.getValue(2));
quatBuff = quat2_inv * quatG2P0_abs / quat2_inv;
_G2P0x = quatBuff.R_component_2();
_G2P0y = quatBuff.R_component_3();
_G2P0z = quatBuff.R_component_4();
// std::cout << "KneeJoint G1P0 :" << _G1P0x << " " << _G1P0y << " " << _G1P0z << std::endl;
// std::cout << "KneeJoint G2P0 :" << _G2P0x << " " << _G2P0y << " " << _G2P0z << std::endl;
checkInitPos(q1,q2);
}
void D1MinusLinearOSI::initializeWorkVectorsForDS(double t, SP::DynamicalSystem ds)
{
// Get work buffers from the graph
VectorOfVectors& ds_work_vectors = *_initializeDSWorkVectors(ds);
// Check dynamical system type
Type::Siconos dsType = Type::value(*ds);
assert(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS || dsType == Type::NewtonEulerDS);
if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds);
lds->init_generalized_coordinates(2); // acceleration is required for the ds
lds->init_inverse_mass(); // invMass required to update post-impact velocity
ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(lds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(lds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(lds->dimension()));
// Update dynamical system components (for memory swap).
lds->computeForces(t, lds->q(), lds->velocity());
lds->swapInMemory();
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds);
neds->init_inverse_mass(); // invMass required to update post-impact velocity
ds_work_vectors.resize(D1MinusLinearOSI::WORK_LENGTH);
ds_work_vectors[D1MinusLinearOSI::RESIDU_FREE].reset(new SiconosVector(neds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE].reset(new SiconosVector(neds->dimension()));
ds_work_vectors[D1MinusLinearOSI::FREE_TDG].reset(new SiconosVector(neds->dimension()));
//Compute a first value of the forces to store it in _forcesMemory
neds->computeForces(t, neds->q(), neds->twist());
neds->swapInMemory();
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::initialize - not implemented for Dynamical system type: " + dsType);
for (unsigned int k = _levelMinForInput ; k < _levelMaxForInput + 1; k++)
{
ds->initializeNonSmoothInput(k);
}
}
/* constructor,
\param a SP::NewtonEulerDS d1, a dynamical system containing the intial position
\param a SP::SiconosVector P0, if (absolutRef) P0 contains the coordinates of the Knee point, in the absolute frame, when d1 is located in the initial position.
else P0 contains the coordinates of the Knee point, in the frame of d1,
ie P0 in the frame of the object, ie G1P0 in the obsolut frame with d1->q=(x,y,z,1,0,0,0).
*/
KneeJointR::KneeJointR(SP::NewtonEulerDS d1, SP::SiconosVector P0, bool absolutRef): NewtonEulerR()
{
_P0.reset(new SiconosVector(3));
*_P0 = *P0;
SP::SiconosVector q1 = d1->q0();
::boost::math::quaternion<double> quat1(q1->getValue(3), q1->getValue(4), q1->getValue(5), q1->getValue(6));
::boost::math::quaternion<double> quat1_inv(q1->getValue(3), -q1->getValue(4), -q1->getValue(5), -q1->getValue(6));
::boost::math::quaternion<double> quatBuff(0, 0, 0, 0);
if(absolutRef)
{
/*quadBuff contains the vector _G1P0 if the object has no orientation.*/
::boost::math::quaternion<double> quatG1P0_abs_init_position(0, _P0->getValue(0) - q1->getValue(0), _P0->getValue(1) - q1->getValue(1), _P0->getValue(2) - q1->getValue(2));
quatBuff = quat1_inv * quatG1P0_abs_init_position * quat1;
_G1P0x = quatBuff.R_component_2();
_G1P0y = quatBuff.R_component_3();
_G1P0z = quatBuff.R_component_4();
_G2P0x = _P0->getValue(0);
_G2P0y = _P0->getValue(1);
_G2P0z = _P0->getValue(2);
}
else
{
_G1P0x = _P0->getValue(0);
_G1P0y = _P0->getValue(1);
_G1P0z = _P0->getValue(2);
/*d2 is look as the ref frame. G2 is the origine, and d2 has no orientation.
Where is P0 in the frame of d2 ?:*/
/*Use initial value of q1 to place P0 in the absolute frame.*/
/*quatG1P0_abs_ without any orientation*/
::boost::math::quaternion<double> quatG1P0_abs_(0, _P0->getValue(0) , _P0->getValue(1) , _P0->getValue(2));
/*quatBuff contains the vector G1P at the initial position*/
quatBuff = quat1 * quatG1P0_abs_ * quat1_inv;
_G2P0x = q1->getValue(0) + quatBuff.R_component_2();
_G2P0y = q1->getValue(1) + quatBuff.R_component_3();
_G2P0z = q1->getValue(2) + quatBuff.R_component_4();
}
// std::cout << "KneeJoint G1P0 :" << _G1P0x << " " << _G1P0y << " " << _G1P0z << std::endl;
// std::cout << "KneeJoint G2P0 :" << _G2P0x << " " << _G2P0y << " " << _G2P0z << std::endl;
SP::SiconosVector q2;
checkInitPos(q1,q2);
}
void LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
DEBUG_BEGIN("LinearOSNS::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)\n");
// Computes matrix _interactionBlocks[inter1][inter1] (and allocates memory if
// necessary) one or two DS are concerned by inter1 . How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::Interaction inter = indexSet->bundle(vd);
// Get osi property from interaction
// We assume that all ds in vertex_inter have the same osi.
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
//SP::OneStepIntegrator Osi = simulation()->integratorOfDS(ds);
OSI::TYPES osiType = Osi->getType();
// At most 2 DS are linked by an Interaction
SP::DynamicalSystem DS1;
SP::DynamicalSystem DS2;
unsigned int pos1, pos2;
// --- Get the dynamical system(s) (edge(s)) connected to the current interaction (vertex) ---
if (indexSet->properties(vd).source != indexSet->properties(vd).target)
{
DEBUG_PRINT("a two DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = indexSet->properties(vd).target;
}
else
{
DEBUG_PRINT("a single DS Interaction\n");
DS1 = indexSet->properties(vd).source;
DS2 = DS1;
// \warning this looks like some debug code, but it gets executed even with NDEBUG.
// may be compiler does something smarter, but still it should be rewritten. --xhub
InteractionsGraph::OEIterator oei, oeiend;
for (std11::tie(oei, oeiend) = indexSet->out_edges(vd);
oei != oeiend; ++oei)
{
// note : at most 4 edges
DS2 = indexSet->bundle(*oei);
if (DS2 != DS1)
{
assert(false);
break;
}
}
}
assert(DS1);
assert(DS2);
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
// --- Check block size ---
assert(indexSet->properties(vd).block->size(0) == inter->nonSmoothLaw()->size());
assert(indexSet->properties(vd).block->size(1) == inter->nonSmoothLaw()->size());
// --- Compute diagonal block ---
// Block to be set in OSNS Matrix, corresponding to
// the current interaction
SP::SiconosMatrix currentInteractionBlock = indexSet->properties(vd).block;
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType;
double h = simulation()->currentTimeStep();
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType = inter->relation()->getType();
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
inter->getExtraInteractionBlock(currentInteractionBlock, workMInter);
unsigned int nslawSize = inter->nonSmoothLaw()->size();
// loop over the DS connected to the interaction.
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
unsigned int sizeDS = ds->dimension();
// get _interactionBlocks corresponding to the current DS
// These _interactionBlocks depends on the relation type.
leftInteractionBlock.reset(new SimpleMatrix(nslawSize, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
DEBUG_EXPR(leftInteractionBlock->display(););
// Computing depends on relation type -> move this in Interaction method?
if (relationType == FirstOrder)
{
rightInteractionBlock.reset(new SimpleMatrix(sizeDS, nslawSize));
inter->getRightInteractionBlockForDS(pos, rightInteractionBlock, workMInter);
if (osiType == OSI::EULERMOREAUOSI)
{
if ((std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGamma() || (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->useGammaForRelation())
{
*rightInteractionBlock *= (std11::static_pointer_cast<EulerMoreauOSI> (Osi))->gamma();
}
}
// for ZOH, we have a different formula ...
if (osiType == OSI::ZOHOSI && indexSet->properties(vd).forControl)
{
*rightInteractionBlock = std11::static_pointer_cast<ZeroOrderHoldOSI>(Osi)->Bd(ds);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
// centralInteractionBlock contains a lu-factorized matrix and we solve
// centralInteractionBlock * X = rightInteractionBlock with PLU
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
inter->computeKhat(*rightInteractionBlock, workMInter, h); // if K is non 0
// integration of r with theta method removed
// *currentInteractionBlock += h *Theta[*itDS]* *leftInteractionBlock * (*rightInteractionBlock); //left = C, right = W.B
//gemm(h,*leftInteractionBlock,*rightInteractionBlock,1.0,*currentInteractionBlock);
*leftInteractionBlock *= h;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
//left = C, right = inv(W).B
}
}
else if (relationType == Lagrangian ||
relationType == NewtonEuler)
{
SP::BoundaryCondition bc;
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
if (d->boundaryConditions()) bc = d->boundaryConditions();
}
if (bc)
{
for (std::vector<unsigned int>::iterator itindex = bc->velocityIndices()->begin() ;
itindex != bc->velocityIndices()->end();
++itindex)
{
// (nslawSize,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(nslawSize));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
DEBUG_PRINT("leftInteractionBlock after application of boundary conditions\n");
DEBUG_EXPR(leftInteractionBlock->display(););
void D1MinusLinearOSI::updateState(const unsigned int level)
{
DEBUG_PRINTF("\n D1MinusLinearOSI::updateState(const unsigned int level) start for level = %i\n",level);
for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
// type of the current DS
Type::Siconos dsType = Type::value(**it);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
/* Lagrangian DS*/
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it);
SP::SiconosMatrix M = d->mass();
SP::SiconosVector v = d->velocity();
DEBUG_PRINT("Position and velocity before update\n");
DEBUG_EXPR(d->q()->display());
DEBUG_EXPR(d->velocity()->display());
/* Add the contribution of the impulse if any */
if (d->p(1))
{
DEBUG_EXPR(d->p(1)->display());
/* copy the value of the impulse */
SP::SiconosVector dummy(new SiconosVector(*(d->p(1))));
/* Compute the velocity jump due to the impulse */
M->PLUForwardBackwardInPlace(*dummy);
/* Add the velocity jump to the free velocity */
*v += *dummy;
}
DEBUG_PRINT("Position and velocity after update\n");
DEBUG_EXPR(d->q()->display());
DEBUG_EXPR(d->velocity()->display());
}
/* NewtonEuler Systems */
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it);
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
SP::SiconosVector v = d->velocity(); // POINTER CONSTRUCTOR : contains new velocity
if (d->p(1))
{
// Update the velocity
SP::SiconosVector dummy(new SiconosVector(*(d->p(1)))); // value = nonsmooth impulse
M->PLUForwardBackwardInPlace(*dummy); // solution for its velocity equivalent
*v += *dummy; // add free velocity
// update \f$ \dot q \f$
SP::SiconosMatrix T = d->T();
SP::SiconosVector dotq = d->dotq();
prod(*T, *v, *dotq, true);
DEBUG_PRINT("\nRIGHT IMPULSE\n");
DEBUG_EXPR(d->p(1)->display());
}
DEBUG_EXPR(d->q()->display());
DEBUG_EXPR(d->velocity()->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("\n D1MinusLinearOSI::updateState(const unsigned int level) end\n");
}
void D1MinusLinearOSI::computeFreeState()
{
DEBUG_PRINT("\n D1MinusLinearOSI::computeFreeState(), start\n");
for (DSIterator it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it)
{
// type of the current DS
Type::Siconos dsType = Type::value(**it);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
// Lagrangian Systems
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (*it);
// get left state from memory
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
DEBUG_EXPR(vold->display());
// get right information
//SP::SiconosMatrix M = d->mass();
SP::SiconosVector vfree = d->velocity(); // POINTER CONSTRUCTOR : contains free velocity
(*vfree) = *(d->workspace(DynamicalSystem::freeresidu));
DEBUG_EXPR(d->workspace(DynamicalSystem::freeresidu)->display());
// d->computeMass();
// M->resetLU();
// M->PLUForwardBackwardInPlace(*vfree);
// DEBUG_EXPR(M->display());
*vfree *= -1.;
*vfree += *vold;
DEBUG_EXPR(vfree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
// NewtonEuler Systems
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (*it);
// get left state from memory
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
DEBUG_EXPR(vold->display());
// get right information
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
SP::SiconosVector vfree = d->velocity(); // POINTER CONSTRUCTOR : contains free velocity
(*vfree) = *(d->workspace(DynamicalSystem::freeresidu));
DEBUG_EXPR(d->workspace(DynamicalSystem::freeresidu)->display());
*vfree *= -1.;
*vfree += *vold;
DEBUG_EXPR(vfree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("D1MinusLinearOSI::computeFreeState(), end\n");
}
void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::NewtonEulerDS d1)
{
double Nx = _Nc->getValue(0);
double Ny = _Nc->getValue(1);
double Nz = _Nc->getValue(2);
double Px = _Pc1->getValue(0);
double Py = _Pc1->getValue(1);
double Pz = _Pc1->getValue(2);
double G1x = d1->q()->getValue(0);
double G1y = d1->q()->getValue(1);
double G1z = d1->q()->getValue(2);
#ifdef NEFC3D_DEBUG
printf("contact normal:\n");
_Nc->display();
printf("contact point :\n");
_Pc1->display();
printf("center of mass :\n");
d1->q()->display();
#endif
assert(_Nc->norm2() >0.0 && "NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Normal vector not consistent ") ;
double t[6];
double * pt = t;
orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
pt = t;
_Mabs_C->setValue(0, 0, Nx);
_Mabs_C->setValue(1, 0, *pt);
_Mabs_C->setValue(2, 0, *(pt + 3));
_Mabs_C->setValue(0, 1, Ny);
_Mabs_C->setValue(1, 1, *(pt + 1));
_Mabs_C->setValue(2, 1, *(pt + 4));
_Mabs_C->setValue(0, 2, Nz);
_Mabs_C->setValue(1, 2, *(pt + 2));
_Mabs_C->setValue(2, 2, *(pt + 5));
#ifdef NEFC3D_DEBUG
printf("_Mabs_C:\n");
_Mabs_C->display();
#endif
_NPG1->zero();
(*_NPG1)(0, 0) = 0;
(*_NPG1)(0, 1) = -(G1z - Pz);
(*_NPG1)(0, 2) = (G1y - Py);
(*_NPG1)(1, 0) = (G1z - Pz);
(*_NPG1)(1, 1) = 0;
(*_NPG1)(1, 2) = -(G1x - Px);
(*_NPG1)(2, 0) = -(G1y - Py);
(*_NPG1)(2, 1) = (G1x - Px);
(*_NPG1)(2, 2) = 0;
// d1->computeMObjToAbs();
SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();
#ifdef NEFC3D_DEBUG
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mobj1_abs:");
Mobj1_abs.display();
#endif
prod(*_NPG1, Mobj1_abs, *_AUX1, true);
#ifdef NEFC3D_DEBUG
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mobj1_abs:");
Mobj1_abs.display();
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
_AUX1->display();
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
_AUX2->display();
#endif
prod(*_Mabs_C, *_AUX1, *_AUX2, true);
#ifdef NEFC3D_DEBUG
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mabs_C:");
_Mabs_C->display();
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
_AUX1->display();
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
_AUX2->display();
#endif
for (unsigned int ii = 0; ii < 3; ii++)
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));
for (unsigned int ii = 0; ii < 3; ii++)
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));
#ifdef NEFC3D_DEBUG
printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, _jahcqT:\n");
_jachqT->display();
SP::SimpleMatrix jaux(new SimpleMatrix(*_jachqT));
jaux->trans();
SP::SiconosVector v(new SiconosVector(3));
SP::SiconosVector vRes(new SiconosVector(6));
v->zero();
v->setValue(0, 1);
prod(*jaux, *v, *vRes, true);
vRes->display();
v->zero();
v->setValue(1, 1);
prod(*jaux, *v, *vRes, true);
vRes->display();
v->zero();
v->setValue(2, 1);
prod(*jaux, *v, *vRes, true);
vRes->display();
#endif
}
void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::NewtonEulerDS d1, SP::NewtonEulerDS d2)
{
double Nx = _Nc->getValue(0);
double Ny = _Nc->getValue(1);
double Nz = _Nc->getValue(2);
double Px = _Pc1->getValue(0);
double Py = _Pc1->getValue(1);
double Pz = _Pc1->getValue(2);
double G1x = d1->q()->getValue(0);
double G1y = d1->q()->getValue(1);
double G1z = d1->q()->getValue(2);
double G2x = d2->q()->getValue(0);
double G2y = d2->q()->getValue(1);
double G2z = d2->q()->getValue(2);
double t[6];
double * pt = t;
orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
pt = t;
_Mabs_C->setValue(0, 0, Nx);
_Mabs_C->setValue(1, 0, *pt);
_Mabs_C->setValue(2, 0, *(pt + 3));
_Mabs_C->setValue(0, 1, Ny);
_Mabs_C->setValue(1, 1, *(pt + 1));
_Mabs_C->setValue(2, 1, *(pt + 4));
_Mabs_C->setValue(0, 2, Nz);
_Mabs_C->setValue(1, 2, *(pt + 2));
_Mabs_C->setValue(2, 2, *(pt + 5));
_NPG1->zero();
(*_NPG1)(0, 0) = 0;
(*_NPG1)(0, 1) = -(G1z - Pz);
(*_NPG1)(0, 2) = (G1y - Py);
(*_NPG1)(1, 0) = (G1z - Pz);
(*_NPG1)(1, 1) = 0;
(*_NPG1)(1, 2) = -(G1x - Px);
(*_NPG1)(2, 0) = -(G1y - Py);
(*_NPG1)(2, 1) = (G1x - Px);
(*_NPG1)(2, 2) = 0;
_NPG2->zero();
(*_NPG2)(0, 0) = 0;
(*_NPG2)(0, 1) = -(G2z - Pz);
(*_NPG2)(0, 2) = (G2y - Py);
(*_NPG2)(1, 0) = (G2z - Pz);
(*_NPG2)(1, 1) = 0;
(*_NPG2)(1, 2) = -(G2x - Px);
(*_NPG2)(2, 0) = -(G2y - Py);
(*_NPG2)(2, 1) = (G2x - Px);
(*_NPG2)(2, 2) = 0;
// d1->computeMObjToAbs();
SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();
prod(*_NPG1, Mobj1_abs, *_AUX1, true);
prod(*_Mabs_C, *_AUX1, *_AUX2, true);
for (unsigned int ii = 0; ii < 3; ii++)
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));
for (unsigned int ii = 0; ii < 3; ii++)
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));
// d2->computeMObjToAbs();
SimpleMatrix& Mobj2_abs = *d2->MObjToAbs();
prod(*_NPG2, Mobj2_abs, *_AUX1, true);
prod(*_Mabs_C, *_AUX1, *_AUX2, true);
for (unsigned int ii = 0; ii < 3; ii++)
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(ii, jj + 6, -_Mabs_C->getValue(ii, jj));
for (unsigned int ii = 0; ii < 3; ii++)
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(ii, jj + 6, -_AUX2->getValue(ii, jj - 3));
}
double D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()
{
DEBUG_BEGIN("\n D1MinusLinearOSI::computeResiduHalfExplicitAccelerationLevel()\n");
double t = _simulation->nextTime(); // end of the time step
double told = _simulation->startingTime(); // beginning of the time step
double h = _simulation->timeStep(); // time step length
SP::OneStepNSProblems allOSNS = _simulation->oneStepNSProblems(); // all OSNSP
SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology();
SP::InteractionsGraph indexSet2 = topo->indexSet(2);
/**************************************************************************************************************
* Step 1- solve a LCP at acceleration level for lambda^+_{k} for the last set indices
* if index2 is empty we should skip this step
**************************************************************************************************************/
DEBUG_PRINT("\nEVALUATE LEFT HAND SIDE\n");
DEBUG_EXPR(std::cout<< "allOSNS->empty() " << std::boolalpha << allOSNS->empty() << std::endl << std::endl);
DEBUG_EXPR(std::cout<< "allOSNS->size() " << allOSNS->size() << std::endl << std::endl);
// -- LEFT SIDE --
DynamicalSystemsGraph::VIterator dsi, dsend;
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
Type::Siconos dsType = Type::value(*ds);
SP::SiconosVector accFree;
SP::SiconosVector work_tdg;
SP::SiconosMatrix Mold;
DEBUG_EXPR((*it)->display());
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
accFree = d->workspace(DynamicalSystem::free); /* POINTER CONSTRUCTOR : will contain
* the acceleration without contact force */
accFree->zero();
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
Mold = d->mass();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
DEBUG_EXPR(Mold->display());
if (! d->workspace(DynamicalSystem::free_tdg))
{
d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
}
work_tdg = d->workspace(DynamicalSystem::free_tdg);
work_tdg->zero();
DEBUG_EXPR(work_tdg->display());
if (d->forces())
{
d->computeForces(told, qold, vold);
DEBUG_EXPR(d->forces()->display());
*accFree += *(d->forces());
}
Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
}
else if(dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
//Mold = d->mass();
assert(!d->mass()->isPLUInversed());
Mold.reset(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
DEBUG_EXPR(Mold->display());
if (! d->workspace(DynamicalSystem::free_tdg))
{
d->allocateWorkVector(DynamicalSystem::free_tdg, d->dimension()) ;
}
work_tdg = d->workspace(DynamicalSystem::free_tdg);
work_tdg->zero();
DEBUG_EXPR(work_tdg->display());
if (d->forces())
{
d->computeForces(told, qold, vold);
DEBUG_EXPR(d->forces()->display());
*accFree += *(d->forces());
}
Mold->PLUForwardBackwardInPlace(*accFree); // contains left (right limit) acceleration without contact force
d->addWorkVector(accFree,DynamicalSystem::free_tdg); // store the value in WorkFreeFree
}
else
{
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
DEBUG_PRINT("accFree contains right limit acceleration at t^+_k with contact force :\n");
DEBUG_EXPR(accFree->display());
DEBUG_PRINT("work_tdg contains right limit acceleration at t^+_k without contact force :\n");
DEBUG_EXPR(work_tdg->display());
}
if (!allOSNS->empty())
{
if (indexSet2->size() >0)
{
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui)
{
inter = indexSet2->bundle(*ui);
inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui));
inter->relation()->computeJacg(told, *inter, indexSet2->properties(*ui));
}
if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged())
{
for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns)
{
(*itOsns)->setHasBeenUpdated(false);
}
}
assert((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]);
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions())) // it should be equivalent to indexSet2
{
DEBUG_PRINT("We compute lambda^+_{k} \n");
(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(told);
DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display());
}
// Note Franck : at the time this results in a call to swapInMem of all Interactions of the NSDS
// So let the simu do this.
//(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->saveInMemory(); // we push y and lambda in Memories
_simulation->nonSmoothDynamicalSystem()->pushInteractionsInMemory();
_simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2);
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
Type::Siconos dsType = Type::value(*ds);
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force
SP::SiconosMatrix Mold = d->mass();
Mold->PLUForwardBackwardInPlace(*dummy);
*accFree += *(dummy);
DEBUG_EXPR(d->p(2)->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
SP::SiconosVector dummy(new SiconosVector(*(d->p(2)))); // value = contact force
SP::SiconosMatrix Mold(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization
DEBUG_EXPR(Mold->display());
Mold->PLUForwardBackwardInPlace(*dummy);
*accFree += *(dummy);
DEBUG_EXPR(d->p(2)->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
}
}
/**************************************************************************************************************
* Step 2 - compute v_{k,1}
**************************************************************************************************************/
DEBUG_PRINT("\n PREDICT RIGHT HAND SIDE\n");
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // contains acceleration without contact force
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// initialize *it->residuFree and predicted right velocity (left limit)
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect
SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity
residuFree->zero();
v->zero();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
*residuFree -= 0.5 * h**accFree;
*v += h**accFree;
*v += *vold;
DEBUG_EXPR(residuFree->display());
DEBUG_EXPR(v->display());
SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1}
*q = *qold;
scal(0.5 * h, *vold + *v, *q, false);
DEBUG_EXPR(q->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free);
// get left state from memory
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0);
// initialize *it->residuFree and predicted right velocity (left limit)
SP::SiconosVector residuFree = ds->workspace(DynamicalSystem::freeresidu); // contains residu without nonsmooth effect
SP::SiconosVector v = d->velocity(); //contains velocity v_{k+1}^- and not free velocity
residuFree->zero();
v->zero();
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(qold->display());
DEBUG_EXPR(vold->display());
*residuFree -= 0.5 * h**accFree;
*v += h**accFree;
*v += *vold;
DEBUG_EXPR(residuFree->display());
DEBUG_EXPR(v->display());
//first step consists in computing \dot q.
//second step consists in updating q.
//
SP::SiconosMatrix T = d->T();
SP::SiconosVector dotq = d->dotq();
prod(*T, *v, *dotq, true);
SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0);
SP::SiconosVector q = d->q(); // POINTER CONSTRUCTOR : contains position q_{k+1}
*q = *qold;
scal(0.5 * h, *dotqold + *dotq, *q, false);
DEBUG_PRINT("new q before normalizing\n");
DEBUG_EXPR(q->display());
//q[3:6] must be normalized
d->normalizeq();
d->computeT();
DEBUG_PRINT("new q after normalizing\n");
DEBUG_EXPR(q->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
/** At this step, we obtain
* \f[
* \begin{cases}
* v_{k,0} = \mbox{\tt vold} \\
* q_{k,0} = qold \\
* F_{k,+} = F(told,qold,vold) \\
* Work_{freefree} = M^{-1}_k (F^+_{k}) \mbox{stored in work_tdg} \\
* Work_{free} = M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in accFree} \\
* R_{free} = -h/2 * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in ResiduFree} \\
* v_{k,1} = v_{k,0} + h * M^{-1}_k (P^+_{2,k}+F^+_{k}) \mbox{stored in v} \\
* q_{k,1} = q_{k,0} + \frac{h}{2} (v_{k,0} + v_{k,1}) \mbox{stored in q} \\
* \end{cases}
* \f]
**/
}
DEBUG_PRINT("\n DECIDE STRATEGY\n");
/** Decide of the strategy impact or smooth multiplier.
* Compute _isThereImpactInTheTimeStep
*/
_isThereImpactInTheTimeStep = false;
if (!allOSNS->empty())
{
for (unsigned int level = _simulation->levelMinForOutput();
level < _simulation->levelMaxForOutput(); level++)
{
_simulation->nonSmoothDynamicalSystem()->updateOutput(_simulation->nextTime(),level);
}
_simulation->updateIndexSets();
SP::Topology topo = _simulation->nonSmoothDynamicalSystem()->topology();
SP::InteractionsGraph indexSet3 = topo->indexSet(3);
if (indexSet3->size() > 0)
{
_isThereImpactInTheTimeStep = true;
DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true;\n");
}
else
{
_isThereImpactInTheTimeStep = false;
DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n");
}
}
/* If _isThereImpactInTheTimeStep = true;
* we recompute residuFree by removing the contribution of the nonimpulsive contact forces.
* We add the contribution of the external forces at the end
* of the time--step
* If _isThereImpactInTheTimeStep = false;
* we recompute residuFree by adding the contribution of the external forces at the end
* and the contribution of the nonimpulsive contact forces that are computed by solving the osnsp.
*/
if (_isThereImpactInTheTimeStep)
{
DEBUG_PRINT("There is an impact in the step. indexSet3->size() > 0. _isThereImpactInTheTimeStep = true\n");
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu);
SP::SiconosVector v = d->velocity();
SP::SiconosVector q = d->q();
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix
//residuFree->zero();
//v->zero();
SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg);
assert(work_tdg);
*residuFree = - 0.5 * h**work_tdg;
d->computeMass();
DEBUG_EXPR(M->display());
if (d->forces())
{
d->computeForces(t, q, v);
*work_tdg = *(d->forces());
DEBUG_EXPR(d->forces()->display());
}
M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force
*residuFree -= 0.5 * h**work_tdg;
DEBUG_EXPR(residuFree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector residuFree = d->workspace(DynamicalSystem::freeresidu);
SP::SiconosVector v = d->velocity();
SP::SiconosVector q = d->q();
SP::SiconosVector qold = d->qMemory()->getSiconosVector(0);
SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // right limit
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
DEBUG_EXPR(M->display());
//residuFree->zero();
v->zero();
SP::SiconosVector work_tdg = d->workspace(DynamicalSystem::free_tdg);
assert(work_tdg);
*residuFree = 0.5 * h**work_tdg;
work_tdg->zero();
if (d->forces())
{
d->computeForces(t, q, v);
*work_tdg += *(d->forces());
}
M->PLUForwardBackwardInPlace(*work_tdg); // contains right (left limit) acceleration without contact force
*residuFree -= 0.5 * h**work_tdg;
DEBUG_EXPR(residuFree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
}
else
{
DEBUG_PRINT("There is no impact in the step. indexSet3->size() = 0. _isThereImpactInTheTimeStep = false;\n");
// -- RIGHT SIDE --
// calculate acceleration without contact force
for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi)
{
if (!checkOSI(dsi)) continue;
SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi);
// type of the current DS
Type::Siconos dsType = Type::value(*ds);
/* \warning the following conditional statement should be removed with a MechanicalDS class */
if ((dsType == Type::LagrangianDS) || (dsType == Type::LagrangianLinearTIDS))
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get right state from memory
SP::SiconosVector q = d->q(); // contains position q_{k+1}
SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity
SP::SiconosMatrix M = d->mass(); // POINTER CONSTRUCTOR : contains mass matrix
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(q->display());
DEBUG_EXPR(v->display());
// Lagrangian Nonlinear Systems
if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
{
d->computeMass();
DEBUG_EXPR(M->display());
if (d->forces())
{
d->computeForces(t, q, v);
*accFree += *(d->forces());
}
}
else
RuntimeException::selfThrow
("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force
DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n");
DEBUG_EXPR(accFree->display());
}
else if (dsType == Type::NewtonEulerDS)
{
SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds);
SP::SiconosVector accFree = d->workspace(DynamicalSystem::free); // POINTER CONSTRUCTOR : contains acceleration without contact force
accFree->zero();
// get right state from memory
SP::SiconosVector q = d->q(); // contains position q_{k+1}
SP::SiconosVector v = d->velocity(); // contains velocity v_{k+1}^- and not free velocity
SP::SiconosMatrix M(new SimpleMatrix(*(d->mass()))); // we copy the mass matrix to avoid its factorization;
DEBUG_EXPR(accFree->display());
DEBUG_EXPR(q->display());
DEBUG_EXPR(v->display());
if (d->forces())
{
d->computeForces(t, q, v);
*accFree += *(d->forces());
}
M->PLUForwardBackwardInPlace(*accFree); // contains right (left limit) acceleration without contact force
DEBUG_PRINT("accFree contains left limit acceleration at t^-_{k+1} without contact force :\n");
DEBUG_EXPR(accFree->display());
}
else
RuntimeException::selfThrow("D1MinusLinearOSI::computeResidu - not yet implemented for Dynamical system type: " + dsType);
}
// solve a LCP at acceleration level only for contacts which have been active at the beginning of the time-step
if (!allOSNS->empty())
{
// for (unsigned int level = _simulation->levelMinForOutput(); level < _simulation->levelMaxForOutput(); level++)
// {
// _simulation->updateOutput(level);
// }
// _simulation->updateIndexSets();
DEBUG_PRINT("We compute lambda^-_{k+1} \n");
InteractionsGraph::VIterator ui, uiend;
SP::Interaction inter;
for (std11::tie(ui, uiend) = indexSet2->vertices(); ui != uiend; ++ui)
{
inter = indexSet2->bundle(*ui);
inter->relation()->computeJach(t, *inter, indexSet2->properties(*ui));
inter->relation()->computeJacg(t, *inter, indexSet2->properties(*ui));
}
if (_simulation->nonSmoothDynamicalSystem()->topology()->hasChanged())
{
for (OSNSIterator itOsns = allOSNS->begin(); itOsns != allOSNS->end(); ++itOsns)
{
(*itOsns)->setHasBeenUpdated(false);
}
}
if (((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->hasInteractions()))
{
(*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->compute(t);
DEBUG_EXPR((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY + 1]->display(););
_simulation->nonSmoothDynamicalSystem()->updateInput(_simulation->nextTime(),2);
}
void MLCPProjectOnConstraints::computeInteractionBlock(const InteractionsGraph::EDescriptor& ed)
{
// Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
// necessary) if inter1 and inter2 have commond DynamicalSystem. How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock start " << std::endl;
#endif
// Get dimension of the NonSmoothLaw (ie dim of the interactionBlock)
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem ds = indexSet->bundle(ed);
SP::Interaction inter1 = indexSet->bundle(indexSet->source(ed));
SP::Interaction inter2 = indexSet->bundle(indexSet->target(ed));
// For the edge 'ds', we need to find relative position of this ds
// in inter1 and inter2 relation matrices (--> pos1 and pos2 below)
// - find if ds is source or target in inter_i
InteractionsGraph::VDescriptor vertex_inter;
// - get the corresponding position
unsigned int pos1, pos2;
// source of inter1 :
vertex_inter = indexSet->source(ed);
VectorOfSMatrices& workMInter1 = *indexSet->properties(vertex_inter).workMatrices;
SP::OneStepIntegrator Osi = indexSet->properties(vertex_inter).osi;
SP::DynamicalSystem tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos1 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos1 = indexSet->properties(vertex_inter).target_pos;
}
// now, inter2
vertex_inter = indexSet->target(ed);
VectorOfSMatrices& workMInter2 = *indexSet->properties(vertex_inter).workMatrices;
tmpds = indexSet->properties(vertex_inter).source;
if (tmpds == ds)
pos2 = indexSet->properties(vertex_inter).source_pos;
else
{
tmpds = indexSet->properties(vertex_inter).target;
pos2 = indexSet->properties(vertex_inter).target_pos;
}
unsigned int index1 = indexSet->index(indexSet->source(ed));
unsigned int index2 = indexSet->index(indexSet->target(ed));
unsigned int sizeY1 = 0;
sizeY1 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter1);
unsigned int sizeY2 = 0;
sizeY2 = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter2);
SP::SiconosMatrix currentInteractionBlock;
assert(index1 != index2);
if (index2 > index1) // upper block
{
// if (! indexSet->properties(ed).upper_block)
// {
// indexSet->properties(ed).upper_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
currentInteractionBlock = indexSet->upper_blockProj[ed];
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock currentInteractionBlock " << std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY1 " << sizeY1 << std::endl;
std::cout << "sizeY2 " << sizeY2 << std::endl;
std::cout << "upper_blockProj " << indexSet->upper_blockProj[ed].get() << " of edge " << ed << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter1->number() << " and interaction " << inter2->number() << std::endl;
// std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
//inter1->display();
// std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
//inter2->display();
#endif
assert(currentInteractionBlock->size(0) == sizeY1);
assert(currentInteractionBlock->size(1) == sizeY2);
}
else // lower block
{
// if (! indexSet->properties(ed).lower_block)
// {
// indexSet->properties(ed).lower_block.reset(new SimpleMatrix(sizeY1, sizeY2));
// }
assert(indexSet->lower_blockProj[ed]->size(0) == sizeY1);
assert(indexSet->lower_blockProj[ed]->size(1) == sizeY2);
currentInteractionBlock = indexSet->lower_blockProj[ed];
}
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock;
RELATION::TYPES relationType1, relationType2;
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
relationType1 = inter1->relation()->getType();
relationType2 = inter2->relation()->getType();
if (relationType1 == NewtonEuler &&
relationType2 == NewtonEuler)
{
assert(inter1 != inter2);
currentInteractionBlock->zero();
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
SP::SimpleMatrix T = neds->T();
SP::SimpleMatrix workT(new SimpleMatrix(*T));
workT->trans();
SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
prod(*workT, *T, *workT2, true);
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
workT2->PLUForwardBackwardInPlace(*rightInteractionBlock);
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
#else
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDSProjectOnConstraints(pos1, leftInteractionBlock);
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDSProjectOnConstraints(pos2, rightInteractionBlock);
rightInteractionBlock->trans();
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#endif
else if (relationType1 == Lagrangian &&
relationType2 == Lagrangian)
{
unsigned int sizeDS = ds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY1, sizeDS));
inter1->getLeftInteractionBlockForDS(pos1, leftInteractionBlock, workMInter1);
Type::Siconos dsType = Type::value(*ds);
if (dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
{
SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds);
if (d->boundaryConditions()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
d->boundaryConditions()->velocityIndices()->begin() ;
itindex != d->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY1,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY1));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : leftInteractionBlock" << std::endl;
leftInteractionBlock->display();
#endif
// inter1 != inter2
rightInteractionBlock.reset(new SimpleMatrix(sizeY2, sizeDS));
inter2->getLeftInteractionBlockForDS(pos2, rightInteractionBlock, workMInter2);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : rightInteractionBlock" << std::endl;
rightInteractionBlock->display();
#endif
// Warning: we use getLeft for Right interactionBlock
// because right = transpose(left) and because of
// size checking inside the getBlock function, a
// getRight call will fail.
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : centralInteractionBlocks " << std::endl;
centralInteractionBlock->display();
#endif
rightInteractionBlock->trans();
if (_useMassNormalization)
{
centralInteractionBlock->PLUForwardBackwardInPlace(*rightInteractionBlock);
//*currentInteractionBlock += *leftInteractionBlock ** work;
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
else
{
prod(*leftInteractionBlock, *rightInteractionBlock, *currentInteractionBlock, false);
}
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeInteractionBlock : currentInteractionBlock" << std::endl;
currentInteractionBlock->display();
#endif
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeInteractionBlock not yet implemented for relation of type " + relationType1);
}
void MLCPProjectOnConstraints::computeDiagonalInteractionBlock(const InteractionsGraph::VDescriptor& vd)
{
SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
SP::DynamicalSystem DS1 = indexSet->properties(vd).source;
SP::DynamicalSystem DS2 = indexSet->properties(vd).target;
SP::Interaction inter = indexSet->bundle(vd);
SP::OneStepIntegrator Osi = indexSet->properties(vd).osi;
unsigned int pos1, pos2;
pos1 = indexSet->properties(vd).source_pos;
pos2 = indexSet->properties(vd).target_pos;
unsigned int sizeY = 0;
sizeY = std11::static_pointer_cast<OSNSMatrixProjectOnConstraints>
(_M)->computeSizeForProjection(inter);
#ifdef MLCPPROJ_DEBUG
std::cout << "\nMLCPProjectOnConstraints::computeDiagonalInteractionBlock" <<std::endl;
std::cout << "indexSetLevel()" << indexSetLevel() << std::endl;
// std::cout << "indexSet :"<< indexSet << std::endl;
// std::cout << "vd :"<< vd << std::endl;
// indexSet->display();
// std::cout << "DS1 :" << std::endl;
// DS1->display();
// std::cout << "DS2 :" << std::endl;
// DS2->display();
#endif
assert(indexSet->blockProj[vd]);
SP::SiconosMatrix currentInteractionBlock = indexSet->blockProj[vd];
#ifdef MLCPPROJ_DEBUG
// std::cout<<"MLCPProjectOnConstraints::computeDiagonalInteractionBlock "<<std::endl;
// currentInteractionBlock->display();
std::cout << "sizeY " << sizeY << std::endl;
std::cout << "blockProj " << indexSet->blockProj[vd].get() << " of edge " << vd << " of size " << currentInteractionBlock->size(0) << " x " << currentInteractionBlock->size(0) << " for interaction " << inter->number() << std::endl;
// std::cout<<"inter1->display() "<< inter1->number()<< std::endl;
//inter1->display();
// std::cout<<"inter2->display() "<< inter2->number()<< std::endl;
//inter2->display();
#endif
assert(currentInteractionBlock->size(0) == sizeY);
assert(currentInteractionBlock->size(1) == sizeY);
if (!_hasBeenUpdated)
computeOptions(inter, inter);
// Computes matrix _interactionBlocks[inter1][inter2] (and allocates memory if
// necessary) if inter1 and inter2 have commond DynamicalSystem. How
// _interactionBlocks are computed depends explicitely on the type of
// Relation of each Interaction.
// Warning: we suppose that at this point, all non linear
// operators (G for lagrangian relation for example) have been
// computed through plug-in mechanism.
// Get the W and Theta maps of one of the Interaction -
// Warning: in the current version, if OSI!=MoreauJeanOSI, this fails.
// If OSI = MOREAU, centralInteractionBlocks = W if OSI = LSODAR,
// centralInteractionBlocks = M (mass matrices)
SP::SiconosMatrix leftInteractionBlock, rightInteractionBlock, leftInteractionBlock1;
// General form of the interactionBlock is : interactionBlock =
// a*extraInteractionBlock + b * leftInteractionBlock * centralInteractionBlocks
// * rightInteractionBlock a and b are scalars, centralInteractionBlocks a
// matrix depending on the integrator (and on the DS), the
// simulation type ... left, right and extra depend on the relation
// type and the non smooth law.
VectorOfSMatrices& workMInter = *indexSet->properties(vd).workMatrices;
currentInteractionBlock->zero();
// loop over the common DS
bool endl = false;
unsigned int pos = pos1;
for (SP::DynamicalSystem ds = DS1; !endl; ds = DS2)
{
assert(ds == DS1 || ds == DS2);
endl = (ds == DS2);
if (Type::value(*ds) == Type::LagrangianLinearTIDS ||
Type::value(*ds) == Type::LagrangianDS)
{
if (inter->relation()->getType() != Lagrangian)
{
RuntimeException::selfThrow(
"MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not of type Lagrangian with a LagrangianDS.");
}
SP::LagrangianDS lds = (std11::static_pointer_cast<LagrangianDS>(ds));
unsigned int sizeDS = lds->getDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock, workMInter);
if (lds->boundaryConditions()) // V.A. Should we do that ?
{
for (std::vector<unsigned int>::iterator itindex =
lds->boundaryConditions()->velocityIndices()->begin() ;
itindex != lds->boundaryConditions()->velocityIndices()->end();
++itindex)
{
// (sizeY,sizeDS));
SP::SiconosVector coltmp(new SiconosVector(sizeY));
coltmp->zero();
leftInteractionBlock->setCol(*itindex, *coltmp);
}
}
// (inter1 == inter2)
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//
// std::cout<<"LinearOSNS : leftUBlock\n";
// work->display();
work->trans();
// std::cout<<"LinearOSNS::computeInteractionBlock leftInteractionBlock"<<endl;
// leftInteractionBlock->display();
if (_useMassNormalization)
{
SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
centralInteractionBlock->PLUForwardBackwardInPlace(*work);
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock);
}
else
{
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
}
//*currentInteractionBlock *=h;
}
else if (Type::value(*ds) == Type::NewtonEulerDS)
{
if (inter->relation()->getType() != NewtonEuler)
{
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - relation is not from NewtonEulerR.");
}
SP::NewtonEulerDS neds = (std11::static_pointer_cast<NewtonEulerDS>(ds));
#ifdef MLCPPROJ_WITH_CT
unsigned int sizeDS = neds->getDim();
SP::SimpleMatrix T = neds->T();
SP::SimpleMatrix workT(new SimpleMatrix(*T));
workT->trans();
SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
prod(*workT, *T, *workT2, true);
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
std::cout << "LinearOSNS : leftUBlock\n";
work->display();
work->trans();
std::cout << "LinearOSNS::computeInteractionBlock workT2" <<std::endl;
workT2->display();
workT2->PLUForwardBackwardInPlace(*work);
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
#else
if (0) //(std11::static_pointer_cast<NewtonEulerR> inter->relation())->_isConstact){
{
// unsigned int sizeDS = neds->getDim();
// SP::SimpleMatrix T = neds->T();
// SP::SimpleMatrix workT(new SimpleMatrix(*T));
// workT->trans();
// SP::SimpleMatrix workT2(new SimpleMatrix(6, 6));
// prod(*workT, *T, *workT2, true);
// leftInteractionBlock1.reset(new SimpleMatrix(sizeY, sizeDS));
// inter->getLeftInteractionBlockForDS(pos, leftInteractionBlock);
// leftInteractionBlock.reset(new SimpleMatrix(1, sizeDS));
// for (unsigned int ii = 0; ii < sizeDS; ii++)
// leftInteractionBlock->setValue(1, ii, leftInteractionBlock1->getValue(1, ii));
//
// SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
// //cout<<"LinearOSNS : leftUBlock\n";
// //work->display();
// work->trans();
// //cout<<"LinearOSNS::computeInteractionBlock workT2"<<endl;
// //workT2->display();
// workT2->PLUForwardBackwardInPlace(*work);
// prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
}
else
{
unsigned int sizeDS = (std11::static_pointer_cast<NewtonEulerDS>(ds))->getqDim();
leftInteractionBlock.reset(new SimpleMatrix(sizeY, sizeDS));
inter->getLeftInteractionBlockForDSProjectOnConstraints(pos, leftInteractionBlock);
// #ifdef MLCPPROJ_DEBUG
// std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case leftInteractionBlock : " << std::endl;
// leftInteractionBlock->display();
// #endif
SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
//cout<<"LinearOSNS sizeY="<<sizeY<<": leftUBlock\n";
//work->display();
work->trans();
prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
// #ifdef MLCPPROJ_DEBUG
// std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock - NewtonEuler case currentInteractionBlock : "<< std::endl;
// currentInteractionBlock->display();
// #endif
}
}
else
RuntimeException::selfThrow("MLCPProjectOnConstraints::computeDiagonalInteractionBlock - ds is not from NewtonEulerDS neither a LagrangianDS.");
#endif
#ifdef MLCPPROJ_DEBUG
std::cout << "MLCPProjectOnConstraints::computeDiagonalInteractionBlock DiaginteractionBlock " << std::endl;
currentInteractionBlock->display();
#endif
// Set pos for next loop.
pos = pos2;
}
}