132 lines
3.1 KiB
C++
132 lines
3.1 KiB
C++
#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;
|
|
}
|
|
|
|
cv::Point
|
|
further_point_from_line(cv::Vec<double,3> line, std::vector<Point> points)
|
|
{
|
|
//distance = abs(a*p.x+b*p.y+c) / sqrt(a^2+b^2)
|
|
double denom = sqrt(line(0)*line(0)+line(1)*line(1));
|
|
Point further = points[0];
|
|
double maxdist = -1;
|
|
std::cout << "--- further" << std::endl;
|
|
for(Point p: points)
|
|
{
|
|
double numerator = abs(line(0)*p.x + line(1)*p.y + line(2));
|
|
double distance = numerator/denom;
|
|
if(distance > maxdist) {
|
|
maxdist = distance;
|
|
further = p;
|
|
}
|
|
}
|
|
return further;
|
|
}
|