sagoma/geometry.cpp

#include <iostream>

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

#include "geometry.h"

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);
}

cv::Vec<double,3>
get_line(Point p, Point q)
{
    Vec<double,3> line; //the three positions are a,b,c as in: ax + by + c = 0
    Point vector = q-p;
    line(0) = - vector.y;
    line(1) = vector.x;
    line(2) = vector.y * p.x - vector.x * p.y;
    assert(is_in_line(line, p));
    assert(is_in_line(line, q));
    return line;
}

bool
is_in_line(cv::Vec<double,3> line, Point p)
{
    return line(0)*p.x + line(1)*p.y + line(2) == 0;
}