geometry.h

#pragma once
#include <cmath>
 
struct point_t {
  double x; 
  double y; 
 
  double dist(const point_t other) const {
    return std::sqrt(std::pow(this->x - other.x, 2) + pow(this->y - other.y, 2));
  }
 
  point_t operator+(const point_t &other) const {
    point_t p{.x = this->x + other.x, .y = this->y + other.y};
    return p;
  }
 
  point_t operator-(const point_t &other) const {
    point_t p{.x = this->x - other.x, .y = this->y - other.y};
    return p;
  }
 
  point_t operator*(double s) const { return {x * s, y * s}; }
  point_t operator/(double s) const { return {x / s, y / s}; }
 
  point_t operator-() const { return {-x, -y}; }
  point_t operator+() const { return {x, y}; }
 
  bool operator==(const point_t &rhs) { return x == rhs.x && y == rhs.y; }
};
 
struct pose_t {
  double x;   
  double y;   
  double rot; 
 
  point_t get_point() { return point_t{.x = x, .y = y}; }
};
 
struct Rect {
  point_t min;
  point_t max;
  static Rect from_min_and_size(point_t min, point_t size) { return {min, min + size}; }
  point_t dimensions() const { return max - min; }
  point_t center() const { return (min + max) / 2; }
  double width() const { return max.x - min.x; }
  double height() const { return max.y - min.y; }
  bool contains(point_t p) const {
    bool xin = p.x > min.x && p.x < max.x;
    bool yin = p.y > min.y && p.y < max.y;
    return xin && yin;
  }
};
 
struct Mat2 {
  double X11, X12;
  double X21, X22;
  point_t operator*(const point_t p) const {
    double outx = p.x * X11 + p.y * X12;
    double outy = p.x * X21 + p.y * X22;
    return {outx, outy};
  }
 
  static Mat2 FromRotationDegrees(double degrees) {
    double rad = degrees * (M_PI / 180.0);
    double c = cos(rad);
    double s = sin(rad);
    return {c, -s, s, c};
  }
};