41 lines
973 B
C++
41 lines
973 B
C++
#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);
|
|
}
|