Verified Commit 292b3011 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: add hasSameIndexes to JointModelTpl

parent a1cab998
......@@ -187,6 +187,19 @@ namespace pinocchio
const JointModelBase<JointModelDerived> & jmodel);
/**
* @brief Check whether JointModelTpl<Scalar,...> has the indexes than another JointModelDerived
*
* @param[in] jmodel_generic The generic joint model containing a variant.
* @param[in] jmodel The other joint modelto compare with
*
* @return True if the two joints have the same indexes.
*/
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel);
//
// Visitors on JointDatas
//
......
......@@ -311,7 +311,7 @@ namespace pinocchio
static bool algo(const JointModelBase<JointModel> & jmodel_lhs,
const JointModelDerived & jmodel_rhs)
{
return jmodel_lhs == jmodel_rhs;
return jmodel_lhs.derived() == jmodel_rhs;
}
};
......@@ -324,6 +324,29 @@ namespace pinocchio
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
struct JointModelHasSameIndexesVisitor
: fusion::JointUnaryVisitorBase< JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived>,bool>
{
typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
template<typename JointModel>
static bool algo(const JointModelBase<JointModel> & jmodel_lhs,
const JointModelDerived & jmodel_rhs)
{
return jmodel_lhs.derived().hasSameIndexes(jmodel_rhs);
}
};
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel)
{
typedef JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo;
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
}
//
// Visitors on JointDatas
//
......
......@@ -43,6 +43,7 @@ void test_joint_methods(JointModelBase<JointModel> & jmodel,
pinocchio::JointModel jma(jmodel);
BOOST_CHECK(jmodel == jma);
BOOST_CHECK(jma == jmodel);
BOOST_CHECK(jma.hasSameIndexes(jmodel));
pinocchio::JointData jda(jdata);
BOOST_CHECK(jda == jdata);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment