const SimpleMatrix SchatzmanPaoliOSI::getWBoundaryConditions(SP::DynamicalSystem ds) { assert(ds && "SchatzmanPaoliOSI::getWBoundaryConditions(ds): ds == NULL."); // return *(WBoundaryConditionsMap[0]); assert(_WBoundaryConditionsMap[ds->number()] && "SchatzmanPaoliOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL."); return *(_WBoundaryConditionsMap[ds->number()]); // Copy !! }
void EulerMoreauOSI::setW(const SiconosMatrix& newValue, SP::DynamicalSystem ds) { // Check if ds is in the OSI if (!OSIDynamicalSystems->isIn(ds)) RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds does not belong to this Integrator ..."); // Check dimensions consistency unsigned int line = newValue.size(0); unsigned int col = newValue.size(1); if (line != col) // Check that newValue is square RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - newVal is not square! "); if (!ds) RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - ds == NULL."); unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. unsigned int dsN = ds->number(); if (line != sizeW) // check consistency between newValue and dynamical system size RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal,ds) - unconsistent dimension between newVal and dynamical system to be integrated "); // Memory allocation for W, if required if (!WMap[dsN]) // allocate a new W if required { WMap[dsN].reset(new SimpleMatrix(newValue)); } else // or fill-in an existing one if dimensions are consistent. { if (line == WMap[dsN]->size(0) && col == WMap[dsN]->size(1)) *(WMap[dsN]) = newValue; else RuntimeException::selfThrow("EulerMoreauOSI - setW: inconsistent dimensions with problem size for given input matrix W"); } }
SP::SimpleMatrix EulerMoreauOSI::W(SP::DynamicalSystem ds) { assert(ds && "EulerMoreauOSI::W(ds): ds == NULL."); // return WMap[0]; // if(WMap[ds]==NULL) // RuntimeException::selfThrow("EulerMoreauOSI::W(ds): W[ds] == NULL."); return WMap[ds->number()]; }
SP::SiconosMatrix EulerMoreauOSI::WBoundaryConditions(SP::DynamicalSystem ds) { assert(ds && "EulerMoreauOSI::WBoundaryConditions(ds): ds == NULL."); // return WBoundaryConditionsMap[0]; // if(WBoundaryConditionsMap[ds]==NULL) // RuntimeException::selfThrow("EulerMoreauOSI::WBoundaryConditions(ds): W[ds] == NULL."); return _WBoundaryConditionsMap[ds->number()]; }
const SimpleMatrix EulerMoreauOSI::getW(SP::DynamicalSystem ds) { int dsN = ds->number(); assert(ds && "EulerMoreauOSI::getW(ds): ds == NULL."); // return *(WMap[0]); assert(WMap[dsN] && "EulerMoreauOSI::getW(ds): W[ds] == NULL."); return *(WMap[dsN]); // Copy !! }
const SimpleMatrix EulerMoreauOSI::getWBoundaryConditions(SP::DynamicalSystem ds) { assert(ds && "EulerMoreauOSI::getWBoundaryConditions(ds): ds == NULL."); // return *(WBoundaryConditionsMap[0]); unsigned int dsN = ds->number(); assert(_WBoundaryConditionsMap[dsN] && "EulerMoreauOSI::getWBoundaryConditions(ds): WBoundaryConditions[ds] == NULL."); return *(_WBoundaryConditionsMap[dsN]); // Copy !! }
const SimpleMatrix SchatzmanPaoliOSI::getW(SP::DynamicalSystem ds) { assert(ds && "SchatzmanPaoliOSI::getW(ds): ds == NULL."); // return *(WMap[0]); unsigned int dsN = ds->number(); assert(WMap[dsN] && "SchatzmanPaoliOSI::getW(ds): W[ds] == NULL."); return *(WMap[dsN]); // Copy !! }
void EulerMoreauOSI::setWPtr(SP::SimpleMatrix newPtr, SP::DynamicalSystem ds) { unsigned int line = newPtr->size(0); unsigned int col = newPtr->size(1); if (line != col) // Check that newPtr is square RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal) - newVal is not square! "); if (!ds) RuntimeException::selfThrow("EulerMoreauOSI::setWPtr(newVal,ds) - ds == NULL."); unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. if (line != sizeW) // check consistency between newValue and dynamical system size RuntimeException::selfThrow("EulerMoreauOSI::setW(newVal) - unconsistent dimension between newVal and dynamical system to be integrated "); WMap[ds->number()] = newPtr; // link with new pointer }
SP::DynamicalSystem Topology::getDynamicalSystem(unsigned int requiredNumber) { DynamicalSystemsGraph::VIterator vi, vdend; SP::DynamicalSystem ds; unsigned int currentNumber; for (std11::tie(vi, vdend) = _DSG[0]->vertices(); vi != vdend; ++vi) { ds = _DSG[0]->bundle(*vi); currentNumber = ds->number(); if (currentNumber == requiredNumber) return ds; } RuntimeException::selfThrow("Topology::getDynamicalSystem(n) ds not found."); return ds; }
void SchatzmanPaoliOSI::display() { OneStepIntegrator::display(); std::cout << "====== SchatzmanPaoliOSI OSI display ======" <<std::endl; DynamicalSystemsGraph::VIterator dsi, dsend; for (std11::tie(dsi, dsend) = _dynamicalSystemsGraph->vertices(); dsi != dsend; ++dsi) { if (!checkOSI(dsi)) continue; SP::DynamicalSystem ds = _dynamicalSystemsGraph->bundle(*dsi); std::cout << "--------------------------------" <<std::endl; std::cout << "--> W of dynamical system number " << ds->number() << ": " <<std::endl; if (_dynamicalSystemsGraph->properties(*dsi).W) _dynamicalSystemsGraph->properties(*dsi).W->display(); else std::cout << "-> NULL" <<std::endl; std::cout << "--> and corresponding theta is: " << _theta <<std::endl; } std::cout << "================================" <<std::endl; }
void EulerMoreauOSI::computeFreeState() { // This function computes "free" states of the DS belonging to this Integrator. // "Free" means without taking non-smooth effects into account. DEBUG_PRINT("EulerMoreauOSI::computeFreeState() starts\n"); double t = simulationLink->nextTime(); // End of the time step double told = simulationLink->startingTime(); // Beginning of the time step double h = t - told; // time step length // Operators computed at told have index i, and (i+1) at t. // Note: integration of r with a theta method has been removed // SiconosVector *rold = static_cast<SiconosVector*>(d.rMemory()->getSiconosVector(0)); // Iteration through the set of Dynamical Systems. // DSIterator it; // Iterator through the set of DS. SP::DynamicalSystem ds; // Current Dynamical System. Type::Siconos dsType ; // Type of the current DS. // XXX to be removed -- xhub Topology& topo = *simulationLink->model()->nonSmoothDynamicalSystem()->topology(); DynamicalSystemsGraph& DSG0 = *topo.dSG(0); for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { ds = *it; // the considered dynamical system // XXX TMP hack -- xhub // we have to iterate over the edges of the DSG0 -> the following won't be necessary anymore // Maurice will do that with subgraph :) DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(ds); VectorOfVectors& workVectors = *DSG0.properties(dsgVD).workVectors; dsType = Type::value(*ds); // Its type SiconosMatrix& W = *WMap[ds->number()]; // Its W EulerMoreauOSI matrix of iteration. // 1 - First Order Non Linear Systems if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS) { // xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold) // with ResiduFree = = M(x - x_k) - h*theta*f(t_{k+1}, x) - h*(1-theta)*f(t_k, x_k) // to be updated at current time: W, f // fold is f at t_k // not time dependant: M FirstOrderNonLinearDS& d = *std11::static_pointer_cast<FirstOrderNonLinearDS>(ds); // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector xold = d->xMemory()->getSiconosVector(0); // xi SiconosVector& x = *d.x(); // x = x_k or x = x_{k+1}^{\alpha} // xfree gets ResiduFree at first SiconosVector& xfree = *workVectors[FirstOrderDS::xfree]; xfree = *workVectors[FirstOrderDS::residuFree]; DEBUG_PRINT("EulerMoreauOSI::computeFreeState xfree <- residuFree\n"); DEBUG_EXPR(xfree.display()); if (_useGamma) { SiconosVector& rold = *d.rMemory()->getSiconosVector(0); double coeff = -h * (1 - _gamma); scal(coeff, rold, xfree, false); // xfree += -h(1-gamma)*rold } // At this point xfree = (ResiduFree - h(1-gamma)*rold) // -> Solve WX = xfree and set xfree = X W.PLUForwardBackwardInPlace(xfree); // at this point, xfree = W^{-1} (ResiduFree - h(1-gamma)*rold) // -> compute real xfree = x - W^{-1} (ResiduFree - h(1-gamma)*rold) xfree *= -1.0; xfree += x; DEBUG_EXPR(xfree.display()); // now the crazy intermediate variables // xPartialNS was updated before this fonction call // It constains either 0 (first Newton iterate) // or g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x SiconosVector& xPartialNS = *workVectors[FirstOrderDS::xPartialNS]; DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS from Interaction\n"); DEBUG_EXPR(xPartialNS.display()); // -> Solve WX = g(x, \lambda, t_{k+1}) - B_{k+1}^{\alpha} \lambda - K_{k+1}^{\alpha} x // and set xPartialNS = X W.PLUForwardBackwardInPlace(xPartialNS); scal(h, xPartialNS, xPartialNS); // compute real xPartialNS = xfree + ... xPartialNS += xfree; DEBUG_PRINT("EulerMoreauOSI::computeFreeState xPartialNS real value\n"); DEBUG_EXPR(xPartialNS.display()); // deltaxForRelation = (\widetilde{K}_{k+1}^{\alpha})^{-1} xPartialNS - x SiconosVector& deltaxForRelation = *workVectors[FirstOrderDS::deltaxForRelation]; deltaxForRelation = xPartialNS; deltaxForRelation -= x; DEBUG_EXPR(deltaxForRelation.display()); // have a look at the end of the DevNotes for this part if (_useGammaForRelation) { if (!(dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS)) RuntimeException::selfThrow("EulerMoreauOSI::computeFreeState - _useGammaForRelation == true is only implemented for FirstOrderLinearDS or FirstOrderLinearTIDS"); deltaxForRelation = xfree; scal(_gamma, deltaxForRelation, deltaxForRelation); SiconosVector& xold = *d.xMemory()->getSiconosVector(0); scal(1.0 - _gamma, xold, deltaxForRelation, false); } // some output DEBUG_EXPR(xfree.display();); DEBUG_EXPR(xPartialNS.display(););
void EulerMoreauOSI::initW(double t, SP::DynamicalSystem ds) { // This function: // - allocate memory for a matrix W // - insert this matrix into WMap with ds as a key if (!ds) RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds == NULL"); if (!OSIDynamicalSystems->isIn(ds)) RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - ds does not belong to the OSI."); unsigned int dsN = ds->number(); if (WMap.find(dsN) != WMap.end()) RuntimeException::selfThrow("EulerMoreauOSI::initW(t,ds) - W(ds) is already in the map and has been initialized."); unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. // Memory allocation for W // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW)); // SP::SiconosMatrix W = WMap[ds]; double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(*ds); // 1 - First order non linear systems if (dsType == Type::FirstOrderNonLinearDS || dsType == Type::FirstOrderLinearDS || dsType == Type::FirstOrderLinearTIDS) { // // Memory allocation for W // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW)); // SP::SiconosMatrix W = WMap[ds]; // W = M - h*_theta* [jacobian_x f(t,x,z)] SP::FirstOrderNonLinearDS d = std11::static_pointer_cast<FirstOrderNonLinearDS> (ds); // Copy M or I if M is Null into W // SP::SiconosMatrix W = WMap[ds]; if (d->M()) // *W = *d->M(); WMap[dsN].reset(new SimpleMatrix(*d->M())); else { //W->eye(); // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW,Siconos::IDENTITY)); WMap[dsN].reset(new SimpleMatrix(sizeW, sizeW)); // Warning if the Jacobian is a sparse matrix WMap[dsN]->eye(); } SP::SiconosMatrix W = WMap[dsN]; // d->computeJacobianfx(t); // Computation of JacxF is not required here // since it must have been done in OSI->initialize, before a call to this function. // Add -h*_theta*jacobian_XF to W scal(-h * _theta, *d->jacobianfx(), *W, false); } else RuntimeException::selfThrow("EulerMoreauOSI::initW - not yet implemented for Dynamical system type :" + dsType); // Remark: W is not LU-factorized nor inversed here. // Function PLUForwardBackward will do that if required. }
void SchatzmanPaoliOSI::computeFreeState() { // This function computes "free" states of the DS belonging to this Integrator. // "Free" means without taking non-smooth effects into account. //double t = simulationLink->nextTime(); // End of the time step //double told = simulationLink->startingTime(); // Beginning of the time step //double h = t-told; // time step length // Operators computed at told have index i, and (i+1) at t. // Note: integration of r with a theta method has been removed // SiconosVector *rold = static_cast<SiconosVector*>(d->rMemory()->getSiconosVector(0)); // Iteration through the set of Dynamical Systems. // DSIterator it; // Iterator through the set of DS. SP::DynamicalSystem ds; // Current Dynamical System. SP::SiconosMatrix W; // W SchatzmanPaoliOSI matrix of the current DS. Type::Siconos dsType ; // Type of the current DS. for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { ds = *it; // the considered dynamical system dsType = Type::value(*ds); // Its type W = WMap[ds->number()]; // Its W SchatzmanPaoliOSI matrix of iteration. //1 - Lagrangian Non Linear Systems if (dsType == Type::LagrangianDS) { // // IN to be updated at current time: W, M, q, v, fL // // IN at told: qi,vi, fLi // // Note: indices i/i+1 corresponds to value at the beginning/end of the time step. // // Index k stands for Newton iteration and thus corresponds to the last computed // // value, ie the one saved in the DynamicalSystem. // // "i" values are saved in memory vectors. // // vFree = v_k,i+1 - W^{-1} ResiduFree // // with // // ResiduFree = M(q_k,i+1)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) // // -- Convert the DS into a Lagrangian one. // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector qold =d->qMemory()->getSiconosVector(0); // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // // --- ResiduFree computation --- // // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)] // // // // vFree pointer is used to compute and save ResiduFree in this first step. // SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d]; // (*vfree)=*(d->workspace(DynamicalSystem::freeresidu)); // // -- Update W -- // // Note: during computeW, mass and jacobians of fL will be computed/ // computeW(t,d); // SP::SiconosVector v = d->velocity(); // v = v_k,i+1 // // -- vfree = v - W^{-1} ResiduFree -- // // At this point vfree = residuFree // // -> Solve WX = vfree and set vfree = X // W->PLUForwardBackwardInPlace(*vfree); // // -> compute real vfree // *vfree *= -1.0; // *vfree += *v; RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType); } // 2 - Lagrangian Linear Systems else if (dsType == Type::LagrangianLinearTIDS) { // IN to be updated at current time: Fext // IN at told: qi,vi, fext // IN constants: K,C // Note: indices i/i+1 corresponds to value at the beginning/end of the time step. // "i" values are saved in memory vectors. // vFree = v_i + W^{-1} ResiduFree // with // ResiduFree = (-h*C -h^2*theta*K)*vi - h*K*qi + h*theta * Fext_i+1 + h*(1-theta)*Fext_i // -- Convert the DS into a Lagrangian one. SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); // Get state i (previous time step) from Memories -> var. indexed with "Old" SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // q_k // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); //v_k // --- ResiduFree computation --- // vFree pointer is used to compute and save ResiduFree in this first step. // Velocity free and residu. vFree = RESfree (pointer equality !!). SP::SiconosVector qfree = d->workspace(DynamicalSystem::free);//workX[d]; (*qfree) = *(d->workspace(DynamicalSystem::freeresidu)); W->PLUForwardBackwardInPlace(*qfree); *qfree *= -1.0; *qfree += *qold; } // 3 - Newton Euler Systems else if (dsType == Type::NewtonEulerDS) { // // IN to be updated at current time: W, M, q, v, fL // // IN at told: qi,vi, fLi // // Note: indices i/i+1 corresponds to value at the beginning/end of the time step. // // Index k stands for Newton iteration and thus corresponds to the last computed // // value, ie the one saved in the DynamicalSystem. // // "i" values are saved in memory vectors. // // vFree = v_k,i+1 - W^{-1} ResiduFree // // with // // ResiduFree = M(q_k,i+1)(v_k,i+1 - v_i) - h*theta*forces(t,v_k,i+1, q_k,i+1) - h*(1-theta)*forces(ti,vi,qi) // // -- Convert the DS into a Lagrangian one. // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // computeW(t,d); // // Get state i (previous time step) from Memories -> var. indexed with "Old" // SP::SiconosVector qold =d->qMemory()->getSiconosVector(0); // SP::SiconosVector vold = d->velocityMemory()->getSiconosVector(0); // // --- ResiduFree computation --- // // ResFree = M(v-vold) - h*[theta*forces(t) + (1-theta)*forces(told)] // // // // vFree pointer is used to compute and save ResiduFree in this first step. // SP::SiconosVector vfree = d->workspace(DynamicalSystem::free);//workX[d]; // (*vfree)=*(d->workspace(DynamicalSystem::freeresidu)); // //*(d->vPredictor())=*(d->workspace(DynamicalSystem::freeresidu)); // // -- Update W -- // // Note: during computeW, mass and jacobians of fL will be computed/ // SP::SiconosVector v = d->velocity(); // v = v_k,i+1 // // -- vfree = v - W^{-1} ResiduFree -- // // At this point vfree = residuFree // // -> Solve WX = vfree and set vfree = X // // std::cout<<"SchatzmanPaoliOSI::computeFreeState residu free"<<endl; // // vfree->display(); // d->luW()->PLUForwardBackwardInPlace(*vfree); // // std::cout<<"SchatzmanPaoliOSI::computeFreeState -WRfree"<<endl; // // vfree->display(); // // scal(h,*vfree,*vfree); // // -> compute real vfree // *vfree *= -1.0; // *vfree += *v; RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeFreeState - not yet implemented for Dynamical system type: " + dsType); } }
void SchatzmanPaoliOSI::computeW(double t, SP::DynamicalSystem ds) { // Compute W matrix of the Dynamical System ds, at time t and for the current ds state. // When this function is called, WMap[ds] is supposed to exist and not to be null // Memory allocation has been done during initW. assert(ds && "SchatzmanPaoliOSI::computeW(t,ds) - ds == NULL"); unsigned int dsN = ds->number(); assert((WMap.find(dsN) != WMap.end()) && "SchatzmanPaoliOSI::computeW(t,ds) - W(ds) does not exists. Maybe you forget to initialize the osi?"); //double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(*ds); SP::SiconosMatrix W = WMap[dsN]; // 1 - Lagrangian non linear systems if (dsType == Type::LagrangianDS) { // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q // SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity // d->computeMass(); // *W = *d->mass(); // if (C) // { // d->computeJacobianqDotForces(t); // scal(-h*_theta, *C,*W,false); // W -= h*_theta*C // } // if (K) // { // d->computeJacobianqForces(t); // scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K; // } RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType); } // 4 - Lagrangian linear systems else if (dsType == Type::LagrangianLinearTIDS) { // Nothing: W does not depend on time. } // === === else if (dsType == Type::NewtonEulerDS) { // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // d->computeJacobianvFL(t); // double thetaFL=_theta; // *(d->luW())=*(d->jacobianvFL()); // scal(h*thetaFL,*(d->jacobianvFL()),*(d->luW()),true); // *(d->luW())+=*(d->massMatrix()); // //cout<<"SchatzmanPaoliOSI::computeW luW before LUFact\n"; // //d->luW()->display(); // d->luW()->PLUFactorizationInPlace(); RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::computeW - not yet implemented for Dynamical system type :" + dsType); // Remark: W is not LU-factorized here. // Function PLUForwardBackward will do that if required. }
void SchatzmanPaoliOSI::initW(double t, SP::DynamicalSystem ds) { // This function: // - allocate memory for a matrix W // - insert this matrix into WMap with ds as a key if (!ds) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds == NULL"); if (!OSIDynamicalSystems->isIn(ds)) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds does not belong to the OSI."); unsigned int dsN = ds->number(); if (WMap.find(dsN) != WMap.end()) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - W(ds) is already in the map and has been initialized."); //unsigned int sizeW = ds->getDim(); // n for first order systems, ndof for lagrangian. // Memory allocation for W // WMap[ds].reset(new SimpleMatrix(sizeW,sizeW)); // SP::SiconosMatrix W = WMap[ds]; double h = simulationLink->timeStep(); Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian non linear systems if (dsType == Type::LagrangianDS) { // SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SP::SiconosMatrix K = d->jacobianqForces(); // jacobian according to q // SP::SiconosMatrix C = d->jacobianqDotForces(); // jacobian according to velocity // WMap[ds].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass(); // SP::SiconosMatrix W = WMap[ds]; // if (C) // scal(-h*_theta, *C,*W,false); // W -= h*_theta*C // if (K) // scal(-h*h*_theta*_theta,*K,*W,false); //*W -= h*h*_theta*_theta**K; // // WBoundaryConditions initialization // if (d->boundaryConditions()) // initWBoundaryConditions(d); RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType); } // 4 - Lagrangian linear systems else if (dsType == Type::LagrangianLinearTIDS) { SP::LagrangianLinearTIDS d = std11::static_pointer_cast<LagrangianLinearTIDS> (ds); SP::SiconosMatrix K = d->K(); SP::SiconosMatrix C = d->C(); WMap[dsN].reset(new SimpleMatrix(*d->mass())); //*W = *d->mass(); SP::SiconosMatrix W = WMap[dsN]; if (C) scal(1 / 2.0 * h * _theta, *C, *W, false); // W += 1/2.0*h*_theta *C if (K) scal(h * h * _theta * _theta, *K, *W, false); // W = h*h*_theta*_theta*K // WBoundaryConditions initialization if (d->boundaryConditions()) initWBoundaryConditions(d); } // === === else if (dsType == Type::NewtonEulerDS) { //WMap[ds].reset(new SimpleMatrix(3,3)); RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::initW - not yet implemented for Dynamical system type :" + dsType); // Remark: W is not LU-factorized nor inversed here. // Function PLUForwardBackward will do that if required. }
void SchatzmanPaoliOSI::updateState(const unsigned int level) { double h = simulationLink->timeStep(); double RelativeTol = simulationLink->relativeConvergenceTol(); bool useRCC = simulationLink->useRelativeConvergenceCriteron(); if (useRCC) simulationLink->setRelativeConvergenceCriterionHeld(true); DSIterator it; SP::SiconosMatrix W; for (it = OSIDynamicalSystems->begin(); it != OSIDynamicalSystems->end(); ++it) { SP::DynamicalSystem ds = *it; W = WMap[ds->number()]; // Get the DS type Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian Systems if (dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS) { // get dynamical system SP::LagrangianDS d = std11::static_pointer_cast<LagrangianDS> (ds); // SiconosVector *vfree = d->velocityFree(); SP::SiconosVector q = d->q(); bool baux = dsType == Type::LagrangianDS && useRCC && simulationLink->relativeConvergenceCriterionHeld(); if (level != LEVELMAX) { // To compute q, we solve W(q - qfree) = p if (d->p(level)) { *q = *d->p(level); // q = p W->PLUForwardBackwardInPlace(*q); } // if (d->boundaryConditions()) // for (vector<unsigned int>::iterator // itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // v->setValue(*itindex, 0.0); *q += * ds->workspace(DynamicalSystem::free); } else *q = * ds->workspace(DynamicalSystem::free); // Computation of the velocity SP::SiconosVector v = d->velocity(); SP::SiconosVector q_k_1 = d->qMemory()->getSiconosVector(1); // q_{k-1} // std::cout << "SchatzmanPaoliOSI::updateState - q_k_1 =" <<std::endl; // q_k_1->display(); // std::cout << "SchatzmanPaoliOSI::updateState - q =" <<std::endl; // q->display(); *v = 1.0 / (2.0 * h) * (*q - *q_k_1); // std::cout << "SchatzmanPaoliOSI::updateState - v =" <<std::endl; // v->display(); // int bc=0; // SP::SiconosVector columntmp(new SiconosVector(ds->getDim())); // if (d->boundaryConditions()) // { // for (vector<unsigned int>::iterator itindex = d->boundaryConditions()->velocityIndices()->begin() ; // itindex != d->boundaryConditions()->velocityIndices()->end(); // ++itindex) // { // _WBoundaryConditionsMap[ds]->getCol(bc,*columntmp); // /*\warning we assume that W is symmetric in the Lagrangian case*/ // double value = - inner_prod(*columntmp, *v); // value += (d->p(level))->getValue(*itindex); // /* \warning the computation of reactionToBoundaryConditions take into // account the contact impulse but not the external and internal forces. // A complete computation of the residue should be better */ // d->reactionToBoundaryConditions()->setValue(bc,value) ; // bc++; // } if (baux) { ds->subWorkVector(q, DynamicalSystem::local_buffer); double aux = ((ds->workspace(DynamicalSystem::local_buffer))->norm2()) / (ds->normRef()); if (aux > RelativeTol) simulationLink->setRelativeConvergenceCriterionHeld(false); } } //2 - Newton Euler Systems else if (dsType == Type::NewtonEulerDS) { // // get dynamical system // SP::NewtonEulerDS d = std11::static_pointer_cast<NewtonEulerDS> (ds); // SP::SiconosVector v = d->velocity(); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate prev v"<<endl; // v->display(); // #endif // /*d->p has been fill by the Relation->computeInput, it contains // B \lambda _{k+1}*/ // *v = *d->p(level); // v = p // d->luW()->PLUForwardBackwardInPlace(*v); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate hWB lambda"<<endl; // v->display(); // #endif // *v += * ds->workspace(DynamicalSystem::free); // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"SchatzmanPaoliOSI::updatestate work free"<<endl; // ds->workspace(DynamicalSystem::free)->display(); // std::cout<<"SchatzmanPaoliOSI::updatestate new v"<<endl; // v->display(); // #endif // //compute q // //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); // // std::cout<<"SchatzmanPaoliOSI::updateState v"<<endl; // // v->display(); // // std::cout<<"SchatzmanPaoliOSI::updateState dotq"<<endl; // // dotq->display(); // SP::SiconosVector q = d->q(); // // -> get previous time step state // SP::SiconosVector dotqold = d->dotqMemory()->getSiconosVector(0); // SP::SiconosVector qold = d->qMemory()->getSiconosVector(0); // // *q = *qold + h*(theta * *v +(1.0 - theta)* *vold) // double coeff = h*_theta; // scal(coeff, *dotq, *q) ; // q = h*theta*v // coeff = h*(1-_theta); // scal(coeff,*dotqold,*q,false); // q += h(1-theta)*vold // *q += *qold; // #ifdef SCHATZMANPAOLI_NE_DEBUG // std::cout<<"new q before normalizing"<<endl; // q->display(); // #endif // //q[3:6] must be normalized // d->normalizeq(); // dotq->setValue(3,(q->getValue(3)-qold->getValue(3))/h); // dotq->setValue(4,(q->getValue(4)-qold->getValue(4))/h); // dotq->setValue(5,(q->getValue(5)-qold->getValue(5))/h); // dotq->setValue(6,(q->getValue(6)-qold->getValue(6))/h); // d->updateT(); RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } else RuntimeException::selfThrow("SchatzmanPaoliOSI::updateState - not yet implemented for Dynamical system type: " + dsType); } }