#include <iostream>

#ifdef HAS_OPENCV3
#include <opencv2/core.hpp> //Any OPENCV3 code
#else
#include <opencv2/core/core.hpp> //Any Opencv2 code
#endif

double dist(cv::Point a, cv::Point b)
{
		return sqrt((double)pow(a.x-b.x,2)+pow(a.y-b.y,2));
}

double inner_angle(cv::Point a, cv::Point middle, cv::Point b)
{
	cv::Point v1 = a - middle;
	cv::Point v2 = b - middle;
	double v1_length = dist(cv::Point(0, 0), v1);
	double v2_length = dist(cv::Point(0, 0), v2);
	return acos( (v1.x*v2.x + v1.y*v2.y) / (v1_length*v2_length) );
}

cv::Point
map_point(double theta, cv::Point origin, cv::Point tomap)
{
	//that's the algorithm: res = Rt*(tomap - origin)
	cv::Vec<double,2> p_as_vector;
	cv::Point p;
	cv::Matx<double,2,2> Rt;

	p = tomap - origin;
	p_as_vector[0] = p.x;
	p_as_vector[1] = p.y;

	Rt(0,0) = cos(theta);
	Rt(0,1) = sin(theta);
	Rt(1,1) = Rt(0,0);
	Rt(1,0) = -Rt(0,1);
	cv::Vec<double,2> res_as_vector = Rt*p_as_vector;
    return cv::Point(res_as_vector);
}