Program listing for file control/src/Simulation/ControlSimulation_impl.hpp

Program listing for file control/src/Simulation/ControlSimulation_impl.hpp#

  1#ifndef ControlSimulation_impl_hpp
  2#define ControlSimulation_impl_hpp
  3
  4#include <utility>
  5
  6#include "SimulationTypeDef.hpp"
  7
  8#include <SiconosConfig.h>
  9#define TO_STR(x) std::to_string(x)
 10
 11static inline std::pair<unsigned, std::string> getNumberOfStates(DynamicalSystemsGraph& DSG0, InteractionsGraph& IG0)
 12{
 13  std::string legend;
 14  DynamicalSystemsGraph::VIterator dsvi, dsvdend;
 15  unsigned nb = 0;
 16  unsigned counter = 0;
 17  for (std::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
 18  {
 19    SiconosVector& x = *DSG0.bundle(*dsvi)->x();
 20    nb += x.size();
 21
 22    std::string nameDS;
 23    if (DSG0.name.hasKey(*dsvi)) {
 24      nameDS = DSG0.name[*dsvi];
 25    } else {
 26      nameDS = "unknownDS" + TO_STR(counter);
 27      ++counter;
 28    }
 29
 30    for (unsigned i = 0; i < x.size(); ++i) {
 31      legend.append(" " + nameDS + "_" + TO_STR(i));
 32    }
 33
 34    if (DSG0.u.hasKey(*dsvi)) {
 35      unsigned sizeU = DSG0.u[*dsvi]->size();
 36      nb += sizeU;
 37      for (unsigned i = 0; i < sizeU; ++i) {
 38        legend.append(" " + nameDS + "_u_" + TO_STR(i));
 39      }
 40    }
 41
 42    if (DSG0.e.hasKey(*dsvi)) {
 43      unsigned sizeE = DSG0.e[*dsvi]->size();
 44      for (unsigned i = 0; i < sizeE; ++i) {
 45        legend.append(" " + nameDS + "_e_" + TO_STR(i));
 46      }
 47      nb += DSG0.e[*dsvi]->size();
 48    }
 49  }
 50
 51  InteractionsGraph::VIterator ivi, ivdend;
 52  counter = 0;
 53  for (std::tie(ivi, ivdend) = IG0.vertices(); ivi != ivdend; ++ivi)
 54  {
 55    std::string nameInter;
 56    if (IG0.name.hasKey(*ivi))
 57    {
 58      nameInter = IG0.name[*ivi];
 59    }
 60    else
 61    {
 62      nameInter = "unknownInteraction" + TO_STR(counter);
 63      ++counter;
 64    }
 65    SiconosVector& y = *IG0.bundle(*ivi)->y(0);
 66    nb += y.size();
 67    for (unsigned i = 0; i < y.size(); ++i)
 68    {
 69      legend.append(" " + nameInter + "_y_" + TO_STR(i));
 70    }
 71
 72    SiconosVector& lambda = *IG0.bundle(*ivi)->lambda(0);
 73    nb += lambda.size();
 74    for (unsigned i = 0; i < lambda.size(); ++i)
 75    {
 76      legend.append(" " + nameInter + "_lambda_" + TO_STR(i));
 77    }
 78  }
 79
 80  return std::make_pair(nb, legend);
 81}
 82
 83
 84static inline unsigned storeAllStates(unsigned indx, unsigned startColumn, DynamicalSystemsGraph& DSG0, InteractionsGraph& IG0, SimpleMatrix& data)
 85{
 86  DynamicalSystemsGraph::VIterator dsvi, dsvdend;
 87  unsigned column = startColumn;
 88  for (std::tie(dsvi, dsvdend) = DSG0.vertices(); dsvi != dsvdend; ++dsvi)
 89  {
 90    unsigned i = column;
 91    SiconosVector& x = *DSG0.bundle(*dsvi)->x();
 92    for (unsigned j = 0; j < x.size(); ++i, ++j)
 93    {
 94      data(indx, i) = x(j);
 95    }
 96    column += x.size();
 97
 98    if (DSG0.u.hasKey(*dsvi))
 99    {
100      SiconosVector& u = *DSG0.u[*dsvi];
101      for (unsigned j = 0; j < u.size(); ++i, ++j)
102      {
103        data(indx, i) = u(j);
104      }
105      column += u.size();
106    }
107
108    if (DSG0.e.hasKey(*dsvi))
109    {
110      SiconosVector& e = *DSG0.e[*dsvi];
111      for (unsigned j = 0; j < e.size(); ++i, ++j)
112      {
113        data(indx, i) = e(j);
114      }
115      column += e.size();
116    }
117
118  }
119
120  InteractionsGraph::VIterator ivi, ivdend;
121  for (std::tie(ivi, ivdend) = IG0.vertices(); ivi != ivdend; ++ivi)
122  {
123    unsigned i = column;
124    SiconosVector& y = *IG0.bundle(*ivi)->y(0);
125    for (unsigned j = 0; j < y.size(); ++i, ++j)
126    {
127      data(indx, i) = y(j);
128    }
129    column += y.size();
130
131    SiconosVector& lambda = *IG0.bundle(*ivi)->lambda(0);
132    for (unsigned j = 0; j < lambda.size(); ++i, ++j)
133    {
134      data(indx, i) = lambda(j);
135    }
136    column += lambda.size();
137  }
138
139  return column;
140}
141
142#endif