void calc (JointData & data,
const Eigen::VectorXd & qs) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
}
//! TODO: we should not use graph_t here, but templates instead...
void transitive_reduction_of_dag(graph_t* out_graph, const graph_t* in_graph)
{
#if 0
typedef graph_traits<graph_t>::vertex_iterator vitr;
/*std::pair<vertex_iter, vertex_iter> vp;
int* ptr = result.data();
for (vp = vertices(S); vp.first != vp.second; ++vp.first)*/
std::pair<vitr, vitr> range;
for (vitr itr = range.first; itr != range.second; ++itr)
for (vitr itr = range.first; itr != range.second; ++itr)
for (vitr itr = range.first; itr != range.second; ++itr)
if edges xy and yz exist
delete edge xy
#endif
#if 0
graph_t trans_cl, in_copy = *in_graph;
transitive_closure(trans_cl, in_copy);
graph_t trans_cl_copy;
typedef property_map<graph_t, vertex_index_t>::type vidx_map;
/* typedef indirect_cmp<vidx_map, std::greater<vidx_map::value_type> > idx_greater;
vidx_map imap1 = get(vertex_index, trans_cl);
idx_greater compare(imap1);
typedef graph_traits<graph_t>::edge_descriptor edge_descr;
std::priority_queue<edge_descr, std::vector<edge_descr>, idx_greater> idx_q;
*/
edge_index_cmp cmp(trans_cl_copy);
std::priority_queue<edge_descr, std::vector<edge_descr>, edgeFirstIdxCompare> idx_q(trans_cl_copy);
/*push all edge into q*/
typename graph_traits<graph_t>::edge_iterator ei, eiend;
for (boost::tie(ei, eiend) = edges(trans_cl_copy); ei != eiend; ++ei)
idx_q.push(*ei);
#endif
}
