position_yaw.h

Go to the documentation of this file.

#pragma once
#include "aerial_autonomy/types/position.h"
#include <aerial_autonomy/common/math.h>

struct PositionYaw : public Position {
  PositionYaw() : Position(), yaw(0) {}
  PositionYaw(Position p, double yaw) : Position(p), yaw(yaw) {}
  PositionYaw(double x, double y, double z, double yaw)
      : Position(x, y, z), yaw(yaw) {}

  Position position() { return Position(x, y, z); }

  void setPosition(const Position &p) {
    x = p.x;
    y = p.y;
    z = p.z;
  }

  double yaw; 

  bool operator==(const PositionYaw &p) const {
    return (Position::operator==(p) && yaw == p.yaw);
  }
  bool operator!=(const PositionYaw &p) const { return !(*this == p); }
  PositionYaw operator+(const PositionYaw &p) const {
    Position p1(this->x, this->y, this->z);
    Position p2(p.x, p.y, p.z);
    return PositionYaw(p1 + p2, this->yaw + p.yaw);
  }
  PositionYaw operator-(const PositionYaw &p) const {
    Position p1(this->x, this->y, this->z);
    Position p2(p.x, p.y, p.z);
    return PositionYaw(p1 - p2, math::angleWrap(this->yaw - p.yaw));
  }
};