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(Point a, Point b)
{
		return sqrt((double)pow(a.x-b.x,2)+pow(a.y-b.y,2));
}

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

CoordinateSystem::CoordinateSystem(double angle, Point a)
{
    theta = angle;
    origin = a;
    precompute();
    assert(map(a) == Point(0,0));
    assert(unmap(Point(0,0)) == a);
}
CoordinateSystem::CoordinateSystem(Point a, Point b)
{
	theta = atan2(b.y-a.y, b.x-a.x);
    origin = a;
    precompute();
    assert(map(a) == Point(0,0));
    assert(unmap(Point(0,0)) == a);
    assert(map(b).y == 0);
    assert(map(b).x > 0);
}

void CoordinateSystem::precompute() {
    double costheta = cos(theta);
    double sintheta = sin(theta);
	R(0,0) = costheta;
	R(0,1) = -sintheta;
	R(1,0) = sintheta;
	R(1,1) = costheta;
	Rt(0,0) = costheta;
	Rt(0,1) = sintheta;
	Rt(1,0) = -sintheta;
	Rt(1,1) = costheta;
}
Point CoordinateSystem::map(Point tomap) {
	//that's the algorithm: res = Rt*(tomap - origin)
	cv::Vec<double,2> p_as_vector;
	Point p;
	p = tomap - origin;
	p_as_vector[0] = p.x;
	p_as_vector[1] = p.y;

	cv::Vec<double,2> res_as_vector = Rt*p_as_vector;
    return Point(res_as_vector);
}
Point CoordinateSystem::unmap(Point tounmap) {
    cv::Vec<double,2> q_as_vector;
    q_as_vector[0] = tounmap.x;
    q_as_vector[1] = tounmap.y;
    Point RQ = Point(R*q_as_vector);
    return Point(RQ + origin);
}

Point
map_point(double theta, Point origin, Point tomap)
{
    auto cs = CoordinateSystem(theta, origin);
    return cs.map(tomap);
}

Point
unmap_point(double theta, Point origin, Point tounmap)
{
    return CoordinateSystem(theta, origin).unmap(tounmap);
}
	//that's the algorithm: res = Rt*(tomap - origin)
cv::Vec<double,3>
get_line(Point p, Point q)
{
    cv::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;
}

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