CartesianProduct.hpp

#ifndef AIKIDO_STATESPACE_COMPOUNDSTATESPACE_HPP_
#define AIKIDO_STATESPACE_COMPOUNDSTATESPACE_HPP_
#include <vector>

#include "aikido/statespace/ScopedState.hpp"
#include "aikido/statespace/StateSpace.hpp"

namespace aikido {
namespace statespace {

AIKIDO_DECLARE_POINTERS(CartesianProduct)

// Defined in detail/CartesianProduct.hpp
template <class>
class CompoundStateHandle;

class CartesianProduct
  : public std::enable_shared_from_this<CartesianProduct>
  , public virtual StateSpace
{
public:
  class State;

  using StateHandle = CompoundStateHandle<State>;
  using StateHandleConst = CompoundStateHandle<const State>;

  using ScopedState = statespace::ScopedState<StateHandle>;
  using ScopedStateConst = statespace::ScopedState<StateHandleConst>;

  using StateSpace::compose;

  explicit CartesianProduct(std::vector<ConstStateSpacePtr> _subspaces);

  ScopedState createState() const;

  ScopedState cloneState(const StateSpace::State* stateIn) const;

  std::size_t getNumSubspaces() const;

  template <class Space = StateSpace>
  std::shared_ptr<const Space> getSubspace(std::size_t _index) const;

  template <class Space = StateSpace>
  typename Space::State* getSubState(State* _state, std::size_t _index) const;

  template <class Space = StateSpace>
  const typename Space::State* getSubState(
      const State* _state, std::size_t _index) const;

  template <class Space = StateSpace>
  typename Space::StateHandle getSubStateHandle(
      State* _state, std::size_t _index) const;

  template <class Space = StateSpace>
  typename Space::StateHandleConst getSubStateHandle(
      const State* _state, std::size_t _index) const;

  // Documentation inherited.
  std::size_t getStateSizeInBytes() const override;

  // Documentation inherited.
  StateSpace::State* allocateStateInBuffer(void* _buffer) const override;

  // Documentation inherited.
  void freeStateInBuffer(StateSpace::State* _state) const override;

  // Documentation inherited.
  void compose(
      const StateSpace::State* _state1,
      const StateSpace::State* _state2,
      StateSpace::State* _out) const override;

  // Documentation inherited
  void getIdentity(StateSpace::State* _state) const override;

  // Documentation inherited
  void getInverse(
      const StateSpace::State* _in, StateSpace::State* _out) const override;

  // Documentation inherited
  std::size_t getDimension() const override;

  // Documentation inherited
  void copyState(
      const StateSpace::State* _source,
      StateSpace::State* _destination) const override;

  void expMap(
      const Eigen::VectorXd& _tangent, StateSpace::State* _out) const override;

  void logMap(
      const StateSpace::State* _in, Eigen::VectorXd& _tangent) const override;

  void print(const StateSpace::State* _state, std::ostream& _os) const override;

private:
  std::vector<ConstStateSpacePtr> mSubspaces;
  std::vector<std::size_t> mOffsets;
  std::size_t mSizeInBytes;
};

class CartesianProduct::State : public StateSpace::State
{
protected:
  friend class CartesianProduct;

  State() = default;

  ~State() = default;
};

} // namespace statespace
} // namespace aikido

#include "detail/CartesianProduct-impl.hpp"

#endif // AIKIDO_STATESPACE_COMPOUNDSTATESPACE_HPP_