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::initW(double t, SP::DynamicalSystem ds, DynamicalSystemsGraph::VDescriptor& dsv) { // This function: // - allocate memory for a matrix W if (!ds) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds == NULL"); if (!(checkOSI(_dynamicalSystemsGraph->descriptor(ds)))) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - ds does not belong to the OSI."); if (_dynamicalSystemsGraph->properties(dsv).W) RuntimeException::selfThrow("SchatzmanPaoliOSI::initW(t,ds) - W(ds) is already in the map and has been initialized."); //unsigned int sizeW = ds->dimension(); // n for first order systems, ndof for lagrangian. // Memory allocation for W double h = _simulation->timeStep(); Type::Siconos dsType = Type::value(*ds); // 1 - Lagrangian non linear systems if (dsType == Type::LagrangianDS) { 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(); _dynamicalSystemsGraph->properties(dsv).W.reset(new SimpleMatrix(*d->mass())); //*W = *d->mass(); SP::SiconosMatrix W = _dynamicalSystemsGraph->properties(dsv).W; 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,dsv); } // === === else if (dsType == Type::NewtonEulerDS) { 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. }