C++のメモ

座標変換自体は，ユーザーが注意してソースコード内に決め打ちをすれば問題なく行える．しかし，バグの温床となりやすい箇所である．逆行列にし忘れた場合や掛ける順番を間違えた場合などである．
コード内決め打ちと比較してパフォーマンスは低下するが座標系のつながりの検証もコーディングすることで，これらのバグを防止することができる．
今回は，Boost Graph Library (BGL)を用いて，より高度な座標系群の記述を試みる．

設計指針

1. 各座標系をnodeとする
2. 双方向グラフとし，向きを定義する
3. エッジには変換行列とその向きをプロパティとして与える

// definition of edge property
struct transformation_prop_t
{
  typedef boost::edge_propoerty_tag kind;
};

// definition of transformation 
template<class T>
struct transform
{
  T matrix;
};

// definition of transformation direction
struct transformation_direction_prop_t
{
  typedef boost::edge_property_tag kind;
};

// definition of coordinate_systems class (T = matrix_type)
template<typename T>
class coordinate_systems
{
public:
  typedef T matrix_type;
  typedef boost::property<boost::vertex_name_t, std::string> vertex_property_t;
  typedef boost::property<transformation_t, transform<matrix_type>*,
    boost::property<transform_direction_type, bool,
    boost::property<boost::edge_weight_t, int>>> edge_property_t;
  typedef boost::adjacency_list<boost::listS, boost::vecS, 
    boost::bidirectionalS, vertex_property_t, edge_property_t> map_type;
  typedef typename map_type::vertex_descriptor coordinate_type;

protected:
  map_type map;

  // get matrix from edge 
  T get_transformation(typename map_type::edge_descriptor e)
  {
    std::cout << "From " << boost::get(boost::vertex_name, map)[e.m_source] << "\t" << 
      "To " << boost::get(boost::vertex_name, map)[e.m_target] << std::endl;

    // forward direction
    if( boost::get(transform_direction_tag_type(), map)[e] )
    {
      return boost::get(transformation_t(), map)[e]->mat;
    }
    // inverse direction
    else
    {
      return matrix_traits<matrix_type>::inverse(boost::get(transformation_t(), map)[e]->mat);
    }
  }
public:
  coordinate_systems(){}

  // add new coordinate system by name
  coordinate_type add_coordinate(std::string name)
  {
    coordinate_type coordinate = add_vertex(map);
    boost::get(boost::vertex_name, map)[coordinate] = name;
    return coordinate;
  }

  // add new transformation between two coordinates
  void add_transform(coordinate_type& from, coordinate_type& to, transform<matrix_type>* t)
  {
    map_type::edge_descriptor e, inv_e;
    bool is_added;

    // two edges are made for forward and inverse direction.
    // both the edges have the same pointer of the transformation, but direction property (bool) is different.
    // forward direction edge
    boost::tie(e, is_added) = add_edge(from, to, map);
    boost::get(transformation_t(), map)[e] = t;
    boost::get(transform_direction_type(), map)[e] = true;

    // inverse direction edge 
    boost::tie(inv_e, is_added) = add_edge(to, from, map);
    boost::get(transformation_t(), map)[inv_e] = t;
    boost::get(transform_direction_type(), map)[inv_e] = false;
  }
  
  // get matrix from a coordinate to another coordinate
  T get_transform(coordinate_type& from, coordinate_type& to)
  {
    std::vector<coordinate_t> parent(boost::num_vertices(map));

    boost::dijkstra_shortest_paths(map, from, boost::predecessor_map(&parent[0]));

    matrix_type m;
    if(parent[to] == to) return m;

    std::deque<coordinate_type> route;
    coordinate_type e = to;
    coordinate_type s = parent[e];
    while(e != from)
    {
      auto edge = boost::edge(s, e, map);
      m = matrix_traits<matrix_t>::multiple(get_transformation(edge.first), m);

      e = s;
      s = parent[s];
    }
    return m;
  }
};

サンプルソース

  coordinate_systems<Matrix4x4> coords;
  auto A_cs = coords.add_coordinate("A");
  auto B_cs = coords.add_coordinate("B");
  auto C_cs = coords.add_coordinate("C");
  auto D_cs = coords.add_coordinate("D");

  transform<Matrix4x4> A_T_B, A_T_C, B_T_D;
  A_T_B.mat.TranslateLocal(10.0,  0.0,  0.0);
  A_T_C.mat.TranslateLocal( 0.0, 10.0,  0.0);
  B_T_D.mat.TranslateLocal( 0.0,  0.0, 10.0);

  coords.add_transform(A_CS, B_CS, &A_T_B);
  coords.add_transform(A_CS, C_CS, &A_T_C);
  coords.add_transform(B_CS, D_CS, &B_T_D);

  std::cout << coords.get_transform(D_cs, C_cs) << std::endl;

※matrix_type は
matrix_traits<matrix_type>::inverse(const matrix_type&)
matrix_traits<matrix_type>::multiple(const matrix_type&, const matrix_type&)
でアダプトする

※いくつかtypo入れてますよと．コピペだけじゃ動かない．まぁmatrix_type実装しないと動かないのだけど．