SO3-impl.hpp

namespace aikido {
namespace statespace {

template <class _QualifiedState>
class SO3StateHandle : public statespace::StateHandle<SO3, _QualifiedState>
{
public:
  using typename statespace::StateHandle<SO3, _QualifiedState>::State;
  using typename statespace::StateHandle<SO3, _QualifiedState>::StateSpace;
  using typename statespace::StateHandle<SO3, _QualifiedState>::QualifiedState;

  using Quaternion = typename State::Quaternion;

  SO3StateHandle()
  {
    // Do nothing
  }

  SO3StateHandle(const StateSpace* _space, QualifiedState* _state)
    : statespace::StateHandle<StateSpace, QualifiedState>(_space, _state)
  {
    // Do nothing
  }

  const Quaternion& getQuaternion()
  {
    return this->getStateSpace()->getQuaternion(this->getState());
  }

  void setQuaternion(const Quaternion& _quaternion)
  {
    this->getStateSpace()->setQuaternion(this->getState(), _quaternion);
  }
};

} // namespace statespace
} // namespace aikido