#ifndef _MATHFUNC_H
#define _MATHFUNC_H

#include <math.h>
#include <vector>
#include <cmath>
#include <iostream>
#include "Eigen/Dense"  
#include "Eigen/LU"  
#include "Eigen/Core"  

#define PI 3.1415926

float DistancePoints(int x1, int y1, int x2, int y2);
float DistancePoints3D(int x1, int y1, int z1, int x2, int y2, int z2);
float DistancePointToLine(int x1, int y1, int x2, int y2, int x0, int y0);
float DistancePointToLine3D(int x1, int y1, int z1, int x2, int y2, int z2, int x0, int y0, int z0);
bool PointBetweenLines(int x1, int y1, int x2, int y2, int x0, int y0,int threshold);
//bool PointBetweenLines(int x1, int y1, int x2, int y2, int x0, int y0);
double AngleOfVectors(float x1, float y1, float x2, float y2);
double AngleOf3DVectors(float x1, float y1, float z1, float x2, float y2, float z2);
std::vector<float> GetRotationVector(std::vector<float> v1, std::vector<float> v2);
std::vector<float> matrix2angle(Eigen::Matrix3d rotateMatrix);
std::vector<float> GetRotationQuat(std::vector<float> Quat1, std::vector<float> Quat2);

#endif // #ifndef _MATHFUNC_H