smoother.h

#ifndef SMOOTHER_H
#define SMOOTHER_H

#include <cmath>
#include <vector>

#include "dynamicvoronoi.h"
#include "node3d.h"
#include "vector2d.h"
#include "helper.h"
#include "constants.h"
namespace HybridAStar {
class Smoother {
 public:
  Smoother() {}

  void smoothPath(DynamicVoronoi& voronoi);

  double tracePath(const Node3D* node, int i = 0, std::vector<Node3D> path = std::vector<Node3D>(), double length = 0);

  std::vector<Node3D> getPath() {return path;}

  Vector2D obstacleTerm(Vector2D xi);

  Vector2D curvatureTerm(Vector2D xi0, Vector2D xi1, Vector2D xi2);

  Vector2D smoothnessTerm(Vector2D xim2, Vector2D xim1, Vector2D xi, Vector2D xip1, Vector2D xip2);

  Vector2D voronoiTerm();

  bool isOnGrid(Vector2D vec) {
    if (vec.getX() >= 0 && vec.getX() < width &&
        vec.getY() >= 0 && vec.getY() < height) {
      return true;
    }
    return false;
  }

 private:
  float kappaMax = 1.f / (Constants::r * 1.1);
  float obsDMax = Constants::minRoadWidth;
  float vorObsDMax = Constants::minRoadWidth;
  float alpha = 0.1;
  float wObstacle = 0.2;
  float wVoronoi = 0;
  float wCurvature = 0;
  float wSmoothness = 0.2;
  DynamicVoronoi voronoi;
  int width;
  int height;
  std::vector<Node3D> path;
};
}
#endif // SMOOTHER_H