C++のメモ

1. 2つの座標系間の変換行列をいちいちpath探索で計算．重複する計算が多い．
2. 方向グラフでさかのぼれるように，A->B, A<-Bの二本のエッジを張っている．(bidirectionalなので内部的には4本)
3. vertex_descriptorを返さないので，外装プロパティを外部クラスで設定できない．
4. vertex_listがvecSなので，remove時にmapの作り直しが生じる．

1. vertexに基準座標系からの行列を保存し，データ更新時に基準座標系からbfs + visitorで全頂点に基準座標系からの変換行列を保存
2. 基準座標系が変更された時，基準座標系から出て行くようにエッジの向きを変更することで座標系間のエッジは一本にした
3. 内部プロパティは必要最小限にし，外装プロパティーを実装できるようにした
4. vertex_list_typeをlistSにした．listS時のsearch方法を調べた．

今回の実装において調べた・理解したGraphについてメモGraph

// vertex property
struct coordinate_property_t
{
  boost::default_color_type color;
  boost::numeric::ublas::bounded_matrix<double, 4, 4> transform;
};

// edge property
struct coordinate_link_property_t
{
  boost::numeric::ublas::bounded_matrix<double, 4, 4> transform;
  bool transform_direction;
};

class coordinate_systems
{
public:
  coordinate_type add_coordinate(const coordinate_type parent);
  void remove_coordinate(const coordinate_type coordinate);
  void set_transform_from_parent(const coordinate_type coordinate, ublas::bounded_matrix<double,4,4> const& matrix);
  ublas::bounded_matrix<double,4,4> const& get_transform_from_parent(const coordinate_type coordinate);

  void set_reference(const coordinate_type new_reference);
  ublas::bounded_matrix<double,4,4> const get_transform_from_reference(const coordinate_type coordinate);

  ublas::bounded_matrix<double,4,4> const get_transform(const coordinate_type from, const coordinate_type to);

private:
  void build_transform();

  typedef boost::adjacency_list<boost::listS, boost::listS, boost::bidirectionalS, coordinate_property_t, coordinate_link_property_t> coordinate_system_t;
  typedef coordinate_system_t::vertex_descriptor coordinate_type;
  typedef coordinate_system_t::edge_descriptor coordinate_link_type;
  typedef std::map<coordinate_type, coordinate_link_type> coordinate_link_reverse_dictionary_t;

  coordinate_system_t world;
  // referenceによってエッジの向きが変わるので，parentとつなぐエッジを保存しておく．
  coordinate_link_reverse_dictionary_t coordinate_link_reverse_dictionary;
  std::chrono::time_point<std::chrono::system_clock> build_time;
  std::chrono::time_point<std::chrono::system_clock> modified_time;
};

現在のグラフはcurrent_referenceがrootのtree構造なので，エッジを貼り直すのはcurrent_reference -> new_reference間のエッジ(ひと通りしかない)．
方向を入れ替えたら，エッジプロパティのdirectionを反転させる．

... <- current_reference -> ... -> new_reference -> ...
... <- current_reference <- ... <- new_reference -> ...

reference起点でbread_first_searchを実行．
tree_edgeに反応するvisitorを設定．

struct transform_builder : public boost::default_bfs_visitor
{
  template <class Edge, class Graph>
  void tree_edge(Edge e, Graph& g)
  {
    typename boost::graph_traits<Graph>::vertex_descriptor u = source(e, g), v = target(e, g);    
    boost::numeric::ublas::bounded_matrix<double, 4, 4> T_ref_u = g[u].transform;
    auto const& link_property = g[e];
    boost::numeric::ublas::bounded_matrix<double, 4, 4> T_u_v = link_property.transform;
    if (!link_property.transform_direction) T_u_v = inverse(T_u_v); // 逆行列を求めるのは別に必要
    // vertex vにT_ref_vを保存．graphがconstなので・・・
    const_cast<boost::numeric::ublas::bounded_matrix<double, 4, 4>&>(g[v].transform) = boost::numeric::ublas::prod(T_ref_u, T_u_v);
  }
};

void coordinate_systems::build_transform()
{
  if (build_time < modified_time)
  {
    build_time = std::chrono::system_clock::now();
    boost::breadth_first_search(world, reference, boost::color_map(boost::get(&coordinate_property_t::color, world)).visitor(transform_builder()));
  }
}