SO2.hpp

#ifndef AIKIDO_STATESPACE_SO2STATESPACE_HPP_
#define AIKIDO_STATESPACE_SO2STATESPACE_HPP_
#include <Eigen/Core>
#include <Eigen/Geometry>

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

namespace aikido {
namespace statespace {

// Defined in detail/SO2-impl.hpp
template <class>
class SO2StateHandle;

class SO2 : virtual public StateSpace
{
public:
  class State;

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

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

  using StateSpace::compose;

  SO2() = default;

  ScopedState createState() const;

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

  double toAngle(const State* state) const;

  void fromAngle(State* state, double angle) const;

  Eigen::Rotation2Dd toRotation(const State* state) const;

  void fromRotation(State* state, const Eigen::Rotation2Dd& rotation) 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* out) 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;
};

class SO2::State final : public StateSpace::State
{
public:
  explicit State(double angle = 0.0);

  ~State() = default;

  double toAngle() const;

  void fromAngle(double angle);

  Eigen::Rotation2Dd toRotation() const;

  void fromRotation(const Eigen::Rotation2Dd& rotation);

private:
  double mAngle;

  friend class SO2;
};

} // namespace statespace
} // namespace aikido

#include "detail/SO2-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_SO2STATESPACE_HPP_