123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136 |
- #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"
- #define likely(x) __builtin_expect(!!(x), 1)
- #define unlikely(x) __builtin_expect(!!(x), 0)
- double dist(Point a, Point b)
- {
- return cv::norm(a-b);
- }
- 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_value(line, p) == 0;
- }
- bool
- is_above_line(cv::Vec<double,3> line, Point p)
- {
- return line_value(line, p) > 0;
- }
- double line_value(cv::Vec<double,3> line, Point p)
- {
- return line(0)*p.x + line(1)*p.y + line(2);
- }
- 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)
- Point further = points[0];
- double maxdist = -1;
- for(Point p: points)
- {
- double numerator = abs(line(0)*p.x + line(1)*p.y + line(2));
- if(numerator > maxdist) {
- maxdist = numerator;
- further = p;
- }
- }
- return further;
- }
|