C++ SiconosMatrix::display方法代码示例

void adjointInput::JacobianXbeta(double t, SiconosVector& xvalue, SP::SiconosMatrix JacXbeta)
{


  JacXbeta->setValue(0, 0, 0.0) ;
  JacXbeta->setValue(0, 1, -1.0 / 2.0) ;
  JacXbeta->setValue(1, 0, 1.0 / 2.0) ;
  JacXbeta->setValue(1, 1, 0.0) ;
#ifdef SICONOS_DEBUG
  std::cout << "JacXbeta\n" << std::endl;;
  JacXbeta->display();
#endif

}

             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(););
      // (inter1 == inter2)
      SP::SiconosMatrix work(new SimpleMatrix(*leftInteractionBlock));
      work->trans();
      SP::SiconosMatrix centralInteractionBlock = getOSIMatrix(Osi, ds);
      DEBUG_EXPR(centralInteractionBlock->display(););
      DEBUG_EXPR_WE(std::cout <<  std::boolalpha << " centralInteractionBlock->isPLUFactorized() = "<< centralInteractionBlock->isPLUFactorized() << std::endl;);
      centralInteractionBlock->PLUForwardBackwardInPlace(*work);
      //*currentInteractionBlock +=  *leftInteractionBlock ** work;
      DEBUG_EXPR(work->display(););
      prod(*leftInteractionBlock, *work, *currentInteractionBlock, false);
      //      gemm(CblasNoTrans,CblasNoTrans,1.0,*leftInteractionBlock,*work,1.0,*currentInteractionBlock);
      //*currentInteractionBlock *=h;
      DEBUG_EXPR(currentInteractionBlock->display(););
      assert(currentInteractionBlock->isSymmetric(1e-10));


    }
    else RuntimeException::selfThrow("LinearOSNS::computeInteractionBlock not yet implemented for relation of type " + relationType);
    // Set pos for next loop.
    pos = pos2;

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());
//.........这里部分代码省略.........

//.........这里部分代码省略.........
      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);

}

//.........这里部分代码省略.........
      {
        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();