constraint-data-generic.hpp

//
// Copyright (c) 2023 INRIA
//

#ifndef __pinocchio_algorithm_constraint_data_generic_hpp__
#define __pinocchio_algorithm_constraint_data_generic_hpp__

#include "pinocchio/algorithm/constraints/fwd.hpp"
#include "pinocchio/algorithm/constraints/constraint-model-base.hpp"
#include "pinocchio/algorithm/constraints/constraint-data-base.hpp"
#include "pinocchio/algorithm/constraints/visitors/constraint-model-visitor.hpp"

namespace pinocchio
{

 template<
 typename _Scalar,
 int _Options,
 template<typename S, int O> class ConstraintCollectionTpl>
 struct traits<ConstraintDataTpl<_Scalar, _Options, ConstraintCollectionTpl>>
 {
 typedef _Scalar Scalar;
 enum
 {
 Options = _Options
 };
 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
 };

 template<
 typename _Scalar,
 int _Options,
 template<typename S, int O> class ConstraintCollectionTpl>
 struct ConstraintDataTpl
 : ConstraintDataBase<ConstraintDataTpl<_Scalar, _Options, ConstraintCollectionTpl>>
 , ConstraintCollectionTpl<_Scalar, _Options>::ConstraintDataVariant
 {
 typedef ConstraintDataBase<ConstraintModelTpl<_Scalar, _Options, ConstraintCollectionTpl>> Base;
 typedef _Scalar Scalar;
 enum
 {
 Options = _Options
 };

 typedef ConstraintCollectionTpl<Scalar, Options> ConstraintCollection;
 typedef typename ConstraintCollection::ConstraintDataVariant ConstraintDataVariant;
 typedef typename ConstraintCollection::ConstraintModelVariant ConstraintModelVariant;

 ConstraintDataTpl()
 : ConstraintDataVariant()
 {
 }

 ConstraintDataTpl(const ConstraintDataVariant & cdata_variant)
 : ConstraintDataVariant(cdata_variant)
 {
 }

 template<typename ContraintDataDerived>
 ConstraintDataTpl(const ConstraintDataBase<ContraintDataDerived> & cdata)
 : ConstraintDataVariant((ConstraintDataVariant)cdata.derived())
 {
 BOOST_MPL_ASSERT(
 (boost::mpl::contains<typename ConstraintDataVariant::types, ContraintDataDerived>));
 }

 ConstraintDataVariant & toVariant()
 {
 return static_cast<ConstraintDataVariant &>(*this);
 }

 const ConstraintDataVariant & toVariant() const
 {
 return static_cast<const ConstraintDataVariant &>(*this);
 }

 template<typename ConstraintDataDerived>
 bool isEqual(const ConstraintDataBase<ConstraintDataDerived> & other) const
 {
 return ::pinocchio::isEqual(*this, other.derived());
 }

 bool isEqual(const ConstraintDataTpl & other) const
 {
 return /*Base::isEqual(other) &&*/ toVariant() == other.toVariant();
 }

 bool operator==(const ConstraintDataTpl & other) const
 {
 return isEqual(other);
 }

 bool operator!=(const ConstraintDataTpl & other) const
 {
 return !(*this == other);
 }
 };

 template<
 typename ConstraintDataDerived,
 typename Scalar,
 int Options,
 template<typename S, int O> class ConstraintCollectionTpl>
 bool operator==(
 const ConstraintDataBase<ConstraintDataDerived> & data1,
 const ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & data2)
 {
 return data2 == data1.derived();
 }

} // namespace pinocchio

#endif // ifndef __pinocchio_algorithm_constraint_data_generic_hpp__