DualQuaternionState.hpp

#pragma once
 
#include "state_representation/space/SpatialState.hpp"
 
namespace state_representation {
class DualQuaternionState : public SpatialState {
private:
  Eigen::Quaterniond primary; 
  Eigen::Quaterniond dual; 
 
public:
  explicit DualQuaternionState();
 
  explicit DualQuaternionState(const std::string& name, const std::string& reference = "world");
 
  DualQuaternionState(const DualQuaternionState& state);
 
  explicit DualQuaternionState(const std::string& name,
                               const Eigen::Quaterniond& primary,
                               const Eigen::Quaterniond& dual,
                               const std::string& reference = "world");
 
  const Eigen::Quaterniond& get_primary() const;
 
  const Eigen::Quaterniond& get_dual() const;
 
  void set_primary(const Eigen::Quaterniond& primary);
 
  void set_dual(const Eigen::Quaterniond& dual);
 
  DualQuaternionState& operator*=(const DualQuaternionState& q);
 
  const DualQuaternionState operator*(const DualQuaternionState& p) const;
 
  const DualQuaternionState conjugate() const;
 
  virtual void initialize();
 
  const DualQuaternionState copy() const;
 
  friend const DualQuaternionState operator*(const float& lambda, const DualQuaternionState& state);
 
  friend const DualQuaternionState exp(const DualQuaternionState& state);
 
  friend std::ostream& operator<<(std::ostream& os, const DualQuaternionState& state);
};
 
inline const Eigen::Quaterniond& DualQuaternionState::get_primary() const {
  return this->primary;
}
 
inline const Eigen::Quaterniond& DualQuaternionState::get_dual() const {
  return this->dual;
}
 
inline void DualQuaternionState::set_primary(const Eigen::Quaterniond& primary) {
  this->set_filled();
  this->primary = primary;
}
 
inline void DualQuaternionState::set_dual(const Eigen::Quaterniond& dual) {
  this->set_filled();
  this->dual = dual;
}
}