newValue/3part/Cyclops/include/GeomUtils.h

/*!
 * \file GeomUtils.h
 * \date 2018/08/24
 *
 * \author Lin, Chi
 * Contact: lin.chi@hzleaper.com
 *
 *
 * \note
*/

#ifndef __GeomUtils_h_
#define __GeomUtils_h_

#include "StdUtils.h"
#include <opencv2/opencv.hpp>
#include <opencv2/opencv_modules.hpp>
#include "CyclopsVersion.h"

namespace GeomUtils {

using namespace cv;

/************************************************************************/
/*                         Miscellaneous                                */
/************************************************************************/

template<typename T> 
inline T distance(T x0, T y0, T z0, T x1, T y1, T z1)
{
	return sqrt(pow(x0 - x1, 2) + pow(y0 - y1, 2) + pow(z0 - z1, 2));
}

template<typename T>
inline T distance(const Point3_<T> &p1, const Point3_<T> & p2)
{
	return distance(p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
}

template<typename T>
inline T distance(T x0, T y0, T x1, T y1)
{
	return sqrt(pow(x0 - x1, 2) + pow(y0 - y1, 2));
}

template<typename T>
inline T distance(const Point_<T>& p1, const Point_<T>& p2)
{
	return distance(p1.x, p1.y, p2.x, p2.y);
}

double distance(Point2f p1, float a1, Point2f p2, float a2);

inline double distance(float x0, float y0, float a0, float x1, float y1, float a1)
{
	return distance(Point2f(x0, y0), a0, Point2f(x1, y1), a1);
}

template<typename T>
inline T distance(const Vec<T, 4>& line)
{
	return distance(line[0], line[1], line[2], line[3]);
}

template<typename T>
inline T distance2(T x0, T y0, T x1, T y1)
{
	return pow(x0 - x1, 2) + pow(y0 - y1, 2);
}

template<typename T>
inline T distance2(const Point_<T>& p1, const Point_<T>& p2)
{
	return distance2(p1.x, p1.y, p2.x, p2.y);
}

template<typename T>
inline T distance2(const Vec<T, 4>& line)
{
	return distance2(line[0], line[1], line[2], line[3]);
}

template<typename T>
inline T manhattanDistance(T x0, T y0, T x1, T y1)
{
	return abs(x0 - x1) + abs(y0 - y1);
}

template<typename T>
inline T hypotApprox(T x, T y)
{
	x = abs(x); y = abs(y);
	T r1 = std::max<T>(x, y);
	T r2 = 0.7071067811865 * (x + y); // sqrt(0.5)
	return saturate_cast<T>(1.04119610015 * std::max<T>(r1, r2)); // (1 + sqrt(4 - 2 * sqrt(2))) / 2
}

template<typename T>
inline Point2f pntsCenter(const vector<Point_<T> >& pnts)
{
	double cx = 0, cy = 0;
	int pntCount = pnts.size();
	for (int i = 0; i < pntCount; ++i) {
		const Point_<T>& pnt = pnts[i];
		cx += pnt.x; cy += pnt.y;
	}
	return Point2f(cx / pntCount, cy / pntCount);
}

template<typename T>
inline Point2f pntsCenter(const vector<Point_<T> >& pnts, int startIdx, int endIdx)
{
	// start and end point is excluded
	int c = endIdx - startIdx - 1;
	double x = 0, y = 0;
	if (c <= 0) {
		x = (pnts[startIdx].x + pnts[endIdx].x) / 2;
		y = (pnts[startIdx].y + pnts[endIdx].y) / 2;
	}
	else {
		for (int i = startIdx + 1; i < endIdx; ++i) {
			const Point_<T>& p = pnts[i];
			x += p.x;
			y += p.y;
		}
		x /= c;
		y /= c;
	}
	return Point2f(x, y);
}

template<typename T>
inline bool isZeroPoint(const Point3_<T>& pnt)
{
	return pnt.x == 0 && pnt.y == 0 && pnt.z == 0;
}

template<typename T>
inline bool isZeroPoint(const Point_<T>& pnt)
{
	return pnt.x == 0 && pnt.y == 0;
}

/************************************************************************/
/*                              Rect                                    */
/************************************************************************/

typedef Rect_<double>   Rectd;
typedef Rect_<float>    Rectf;

template<typename _Ty>
bool isImageRoiRectValid(const Rect_<_Ty>& rect)
{
    return rect.x >= 0 && rect.y >= 0 && rect.width > 0 && rect.height > 0;
}


template<typename _Ty>
string rect2str(const Rect_<_Ty>& rect)
{
    stringstream ss;
    ss << rect.x << ", " << rect.y << ", " << rect.width << ", " << rect.height;
    return ss.str();
}

template<typename _Ty>
Rect_<_Ty> rectFromVec(vector<_Ty>& vec)
{
    if (vec.size() < 4)
    {
        return Rect_<_Ty>();
    }
    Rect_<_Ty> rect;
    rect.x = vec[0];
    rect.y = vec[1];
    rect.width = vec[2];
    rect.height = vec[3];
    return rect;
}

template<typename _Point>
Rect genRectCenRadius(_Point cen, int radius)
{
    return Rect(cen.x - radius,
        cen.y - radius,
        radius * 2 + 1,
        radius * 2 + 1);
}
template<typename _Point>
Rect genRectLeftTopRadius(_Point p, int radius)
{
    return Rect(p.x,
        p.y,
        radius * 2,
        radius * 2);
}

template<typename _Point>
Rect genRectCenRadius(const vector<_Point>& ptVec, int radius)
{
    Rect br = cv::boundingRect(ptVec);

    return Rect(br.x - radius, br.y - radius, br.width + 2 * radius, br.height + 2 * radius);
}

Rect loadRect(std::string filepath);

Rect rectInImage(const Mat& img, const Point& pt, const Size& rectSize,
    int xPadding = 0, int yPadding = 0);

Rect extRectTopLeftFix(const Rect& r, int w);

Rect extRectCenFix(const Rect& r, int w);

int intersectRatio(RotatedRect& rect1, RotatedRect& rect2, double& dRatio1, double& dRatio2);

CYCLOPS_GEOM_DLLSPEC void rotatedRect2PtVec(const RotatedRect& rr, vector<Point2f>& ptVec);

inline Point2f rotateRectNthPnt(const RotatedRect& rr, int i)
{
	_ASSERTE(i >= 0 && i < 4);
	if (i >= 4 || i < 0) return Point2f();
	vector<Point2f> ptVec;
	rotatedRect2PtVec(rr, ptVec);
	return ptVec[i];
}

template<typename _Ty, typename _Tx>
inline vector<Point_<_Ty> > rect2PtVec(const Rect_<_Tx>& rect)
{
	vector<Point_<_Ty> > ret(4, rect.tl());
	ret[1].x += rect.width;
	ret[2].x += rect.width;
	ret[2].y += rect.height;
	ret[3].y += rect.height;

	return ret;
}

inline void rect2PtVec(const Rect& br, vector<Point2f>& ptVec)
{
	ptVec = rect2PtVec<float, int>(br);
}

bool isPntInRotatedRect(const RotatedRect& rr, const Point2f& pnt);
bool isPntInRotatedRect(const vector<Point2f>& ptVec, const Point2f& pnt);

template<typename _Ty>
inline Rect_<_Ty> scaleRect(const Rect_<_Ty>& r, _Ty val)
{
	Rect_<_Ty> ret = r;
	ret.x -= val;
	ret.y -= val;
	ret.width += val * 2;
	ret.height += val * 2;
	return ret;
}

inline Rect2i scaleRect(const Rect2i& r, cv::Size orgSize, cv::Size dstSize)
{
	float xScale = (float)dstSize.width / orgSize.width;
	float yScale = (float)dstSize.height / orgSize.height;

	int x = r.tl().x*xScale;
	int y = r.tl().y*yScale;
	int xend = r.br().x*xScale;
	int yend = r.br().y*yScale;
	Rect rescalerect(Point2i(x, y), Point2i(xend, yend));

	rescalerect &= Rect(0, 0, dstSize.width, dstSize.height);
	return rescalerect;
}


template<typename _Ty>
inline Rect_<_Ty> cropRect(const Rect_<_Ty>& r, const Mat& img)
{
	Rect_<_Ty> ret = r;
	_Ty x1 = ret.x + ret.width, y1 = ret.y + ret.height;
	if (ret.x < 0) ret.x = 0;
	if (ret.y < 0) ret.y = 0;
	if (x1 > img.cols) x1 = img.cols;
	if (y1 > img.rows) y1 = img.rows;
	ret.width = x1 - ret.x;
	ret.height = y1 - ret.y;
	return ret;
}

template<typename _Ty>
inline Rect2f boundingRect2fFromPtVec(const vector<Point_<_Ty> >& ptVec)
{
	size_t ptCount = ptVec.size();
	if (ptCount == 0) return Rect();
	const Point_<_Ty>& firstPnt = ptVec[0];
	_Ty minX = firstPnt.x, maxX = firstPnt.x, minY = firstPnt.y, maxY = firstPnt.y;
	for (int i = 1; i < ptCount; ++i) {
		const Point_<_Ty>& pnt = ptVec[i];
		if (pnt.x < minX) minX = pnt.x;
		else if (pnt.x > maxX) maxX = pnt.x;
		if (pnt.y < minY) minY = pnt.y;
		else if (pnt.y > maxY) maxY = pnt.y;
	}

	return Rect2f(minX, minY, maxX - minX, maxY - minY);
}

template<typename _Ty>
inline Point2f tl2fFromPtVec(const vector<Point_<_Ty> >& ptVec)
{
	size_t ptCount = ptVec.size();
	if (ptCount == 0) return Point2f();
	const Point_<_Ty>& firstPnt = ptVec[0];
	_Ty minX = firstPnt.x, minY = firstPnt.y;
	for (int i = 1; i < ptCount; ++i) {
		const Point_<_Ty>& pnt = ptVec[i];
		if (pnt.x < minX) minX = pnt.x;
		if (pnt.y < minY) minY = pnt.y;
	}

	return Point2f(minX, minY);
}

inline Rect boundingRectFromDiagPnts(float x0, float y0, float x1, float y1)
{
	Rect r(floor(x0), floor(y0), ceil(x1), ceil(y1));
	r.width -= r.x - 1;
	r.height -= r.y - 1;
	return r;
}

template<typename _Ty>
inline Rect boundingRectFromPtVec(vector<Point_<_Ty> >& ptVec)
{
	Rect2f r = boundingRect2fFromPtVec(ptVec);
	return boundingRectFromDiagPnts(r.x, r.y, r.x + r.width, r.y + r.height);
}

inline Rect boundingRectFromRR(const RotatedRect& rr)
{
	vector<Point2f> pnts;
	rotatedRect2PtVec(rr, pnts);
	return boundingRectFromPtVec(pnts);
}

template<typename _Ty>
inline Rect_<_Ty> resizeRect(const Rect_<_Ty>& r, float w, float h)
{
	Rect_<_Ty> newr;
	newr.width = w;
	newr.height = h;
	newr.x = r.x + r.width / 2. - w / 2.;
	newr.y = r.y + r.height / 2. - h / 2.;
	return newr;
}

/************************************************************************/
/*                           Intersection                               */
/************************************************************************/
enum IntersectType {
	IntersectUnbounded = 0,
	IntersectPartialBounded = 1,
	IntersectBounded = 2,
	IntersectPartialBoundedRight = 3,
	IntersectNone = 4
};

CYCLOPS_GEOM_DLLSPEC bool intersection(Point2f o1, Point2f p1, Point2f o2, Point2f p2,
	Point2f &r, int intersectType = IntersectUnbounded);
CYCLOPS_GEOM_DLLSPEC bool intersection(const Vec4f& l0, const Vec4f& l1, Point2f& r,
	int intersectType = IntersectUnbounded);
CYCLOPS_GEOM_DLLSPEC bool intersection(const Vec4f& l, const vector<Point2f>& vertexes, vector<Point2f>& interPtVec,
	int intersectType = IntersectUnbounded);

CYCLOPS_GEOM_DLLSPEC int intersection(const Vec4f& line, const Vec3f& circle, Point2f& r1, Point2f& r2);
CYCLOPS_GEOM_DLLSPEC int intersection(const Vec4d& line, const Vec3d& circle, Point2d& r1, Point2d& r2);

CYCLOPS_GEOM_DLLSPEC int intersection(const Vec3f& circle1, const Vec3f& circle2, Point2f& r1, Point2f& r2);
CYCLOPS_GEOM_DLLSPEC int intersection(const Vec3d& circle1, const Vec3d& circle2, Point2d& r1, Point2d& r2);

template<typename T>
inline bool isOnLineSegment(const Point_<T>& pt, const Vec<T, 4>& line)
{
	T dx0 = pt.x - line.val[0];
	T dy0 = pt.y - line.val[1];
	if (abs(dx0) > abs(dy0)) {
		T dx1 = line.val[2] - pt.x;
		if (dx0 >= 0 && dx1 >= 0 || dx0 <= 0 && dx1 <= 0) {
			return true;
		}
		else {
			return false;
		}
	}
	else {
		T dy1 = line.val[3] - pt.y;
		if (dy0 >= 0 && dy1 >= 0 || dy0 <= 0 && dy1 <= 0) {
			return true;
		}
		else {
			return false;
		}
	}
}

Point2f getNearestIntersectionOfMultiLines(const vector<Vec4f>& lines);
Point2f getNearestIntersectionOfMultiLines(const vector<Point2f>& linePnts, const vector<Vec2f>& lineNorms);
Point2f getNearestIntersectionOfMultiLines(const vector<Point2f>& linePnts, const vector<float>& lineAngles, bool useDegree = true);

CYCLOPS_GEOM_DLLSPEC Point2f pedalRoot(const Point2f& pnt, const Vec4f& line);
CYCLOPS_GEOM_DLLSPEC float pedalDistance(const Point2f& pnt, const Vec4f& line);

CYCLOPS_GEOM_DLLSPEC void nearestPntToPoly(const Point2f& pnt, const vector<Point2f>& poly, int& mxIdx, double& mxDist, bool getMin);
CYCLOPS_GEOM_DLLSPEC Point2f nearestRootToPoly(const Point2f& pnt, const vector<Point2f>& poly, int mxIdx, double mxDist);

CYCLOPS_GEOM_DLLSPEC bool nearestPntPairInPolys(const vector<Point2f>& poly1, const vector<Point2f>& poly2,
	int& mxIdx1, int& mxIdx2, bool getMin);

CYCLOPS_GEOM_DLLSPEC void maxDisPolyToPoly(const vector<Point2f>& poly1, const vector<Point2f>& poly2,
	Point2f& pnt1, Point2f& pnt2);

enum {
	CALIPERS_MAXHEIGHT = 0,
	CALIPERS_MINAREARECT = 1,
	CALIPERS_MAXDIST = 2,
	CALIPERS_CSLINE = 3
};

void getMaxDisByRotCaliper(const vector<Point2f>& hullVec,
	int &index1,
	int &index2,
	float * maxDis = nullptr);

void csVerticeByRotCaliper(const vector<Point2f>& hullVec1,
	const vector<Point2f>& hullVec2,
	vector<int>& sepIndexPoly1,
	vector<int>& sepIndexPoly2);
CYCLOPS_GEOM_DLLSPEC bool slicePolyToPnt(vector<Point2f>& poly, const Point2f& pnt); // remove points in polygon's backside to the given point
CYCLOPS_GEOM_DLLSPEC bool slicePolytoLine(vector<Point2f>& poly, const Vec4f& line); // remove points in polygon's backside to the given line
CYCLOPS_GEOM_DLLSPEC bool slicePolyToPoly(vector<Point2f>& poly1, vector<Point2f>& poly2); // remove the outside parts of the two polygons

/************************************************************************/
/*                               Angle                                  */
/************************************************************************/

inline float degreeToRadian(float d)
{
	return (float)(d * CV_PI / 180.);
}

inline float radianToDegree(float r)
{
	return (float)(r * 180. / CV_PI);
}

inline float normAngle(float x, float y)
{
	float angle = atan2f(y, x);
	return radianToDegree(angle);
}

// convert angle to [0, 360]
// call this if you are sure the input angle is in [-360, 360]
inline float normAngleQuick(float angle)
{
	return angle < 0 ? angle + 360 : angle;
}

// convert angle to [0, 360]
inline float normAngle(float angle)
{
	float ret = fmod(angle, 360);
	return ret < 0 ? ret + 360 : ret;
}

// convert angle to [-180, 180]
// call this if you are sure the input angle is in [-360, 360]
inline float normAngle180Quick(float angle)
{
	return angle > 180 ? angle - 360 : (angle < -180 ? angle + 360 : angle);
}

// convert angle to [-180, 180]
inline float normAngle180(float angle)
{
	float ret = fmod(angle, 360);
	return normAngle180Quick(ret);
}

// convert angle to [-90, 90]
inline float normAngle90(float angle)
{
	float ret = fmod(angle, 180);
	return ret > 90 ? ret - 180 : (ret < -90 ? ret + 180 : ret);
}

inline float diffAngle(float angle1, float angle2)
{
	float diff = abs(angle1 - angle2);
	return diff > 180 ? 360 - diff : diff;
}

inline float diffAngle2(float angle1, float angle2)
{
	float r1 = degreeToRadian(angle1);
	Vec2f vp(cos(r1), sin(r1));
	float r2 = degreeToRadian(angle2);
	Vec2f vpp(cos(r2), sin(r2));

	return vp.dot(vpp);
}

inline float diffAngleRaw(float angle1, float angle2)
{
	return diffAngle(normAngle(angle1), normAngle(angle2));
}

inline float bisectAngle(float angle1, float angle2)
{
	float diff = abs(angle1 - angle2);
	return diff < 180 ? (angle1 + angle2) / 2 : (angle1 + angle2 + 360) / 2;
}

inline float getLineAngle(const Vec4f& l)
{
	float r = atan2(l[1] - l[3], l[0] - l[2]);
	return radianToDegree(r);
}

// convert angle to [0, 90], indicate Horizontal or Vertical
inline float getNorm90LineAngle(const Vec4f& l)
{
	float r = atan2(abs(l[1] - l[3]), abs(l[0] - l[2]));
	return radianToDegree(r);
}
inline float getPointAngle(const Point2f& p)
{
	return fastAtan2(p.y, p.x);
}
inline Point2f getPointAngleNormalized(const Point2f& p)
{
	double n = norm(p);
	return n < DBL_EPSILON ? p : p / n;
}

inline float getCrossAngle(const Point2f& p)
{
	Point2f tarDir(0, 1.0);
	float dx = p.dot(tarDir);
	float dy = p.cross(tarDir);
	return fastAtan2(dy, dx);
}

inline float getRayAngle(const Point2f& ori, const Point2f& pnt)
{
	Point2f d = pnt - ori;
	return radianToDegree(atan2(d.y, d.x));
}

inline float getInterAngle(const Point2f& ori, const Point2f& p1, const Point2f& p2)
{
	float a1 = getRayAngle(ori, p1);
	float a2 = getRayAngle(ori, p2);
	float diff = abs(a1 - a2);
	return diff > 180 ? 360 - diff : diff;
}

inline float getInterAngle360(const Point2f& ori, const Point2f& p1, const Point2f& p2)
{
	float a1 = getRayAngle(ori, p1);
	float a2 = getRayAngle(ori, p2);
	float diff = a1 - a2;
	return diff < 0 ? 360 + diff : diff;
}

inline Vec2f getAngleVec(float a)
{
	float r = degreeToRadian(a);
	return Vec2f(cos(r), sin(r));
}

// @param angles a collection of angles in [-180, 180] or [0, 360]
// @param rngStart start of search range
// @param rngEnd end of search range, should not be more than 360 degree away from rngStart
// @param binSize bin size of analysis histogram
// @param pWeights optional weights for angles
// @return major angle in [-360, 360]
float getMajorAngle(const vector<float>& angles, float rngStart = 0, float rngEnd = 360,
	float binSize = 10, const vector<float>* pWeights = nullptr);

// precise cosine
double cosd(double angle);
double sind(double angle);

/************************************************************************/
/*                            Transform                                 */
/************************************************************************/

DEPRECATED void transPoints(vector<Point2d>& vec, const Mat& mat);
DEPRECATED void transPoints(vector<Point2d>& vec, const Matx33d& mat);
DEPRECATED void transPoints(const vector<Point2f>& vec, vector<Point2f>& oVec, const Matx23f& mat);

void getRotationMatrix23f(float* data, Point2f center, float angle, float scale,
	float newCenterX, float newCenterY);

inline void getRotationMatrix23f(float* data, Point2f center, float angle, float scale) {
	return getRotationMatrix23f(data, center, angle, scale, center.x, center.y);
}

inline Matx23f getRotationMatrix23f(Point2f center, float angle, float scale,
	float newCenterX, float newCenterY) {
	Matx23f rotMat;
	getRotationMatrix23f(rotMat.val, center, angle, scale, newCenterX, newCenterY);
	return rotMat;
}

inline Matx23f getRotationMatrix23f(Point2f center, float angle, float scale) {
	return getRotationMatrix23f(center, angle, scale, center.x, center.y);
}

template<typename T, typename S>
inline void getRotationMatrix23(T* data, Point_<S> center, double angle, double scale,
	double newCenterX, double newCenterY)
{
	double a_pi = degreeToRadian(angle);
	double alpha = cos(a_pi)*scale;
	double beta = -sin(a_pi)*scale;
	data[0] = saturate_cast<T>(alpha);
	data[1] = saturate_cast<T>(beta);
	data[2] = saturate_cast<T>(-alpha * center.x - beta * center.y + newCenterX); // move to a new center
	data[3] = saturate_cast<T>(-beta);
	data[4] = saturate_cast<T>(alpha);
	data[5] = saturate_cast<T>(beta * center.x - alpha * center.y + newCenterY);  // move to a new center
}

template<typename T, typename S>
inline void getRotationMatrix34f(T* data, Point3_<S> center, Point3_<S> angle, Point3_<S> offset)
{
	float r = degreeToRadian(angle.x);
	float sinv = sin(r);
	float cosv = cos(r);
	Mat rx = (Mat_<T>(3, 3) << 1, 0, 0, 0, cosv, -sinv, 0, sinv, cosv);
	r = degreeToRadian(angle.y);
	sinv = sin(r);
	cosv = cos(r);
	Mat ry = (Mat_<T>(3, 3) << cosv, 0, sinv, 0, 1, 0, -sinv, 0, cosv);
	r = degreeToRadian(angle.z);
	sinv = sin(r);
	cosv = cos(r);
	Mat rz = (Mat_<T>(3, 3) << cosv, -sinv, 0, sinv, cosv, 0, 0, 0, 1);
	Mat rMat = rz * ry * rx;
	T* pdata = (T*)rMat.data;
	data[0] = pdata[0], data[1] = pdata[1], data[2] = pdata[2];
	data[3] = center.x - data[0] * center.x - data[1] * center.y - data[2] * center.z + offset.x;
	data[4] = pdata[3], data[5] = pdata[4], data[6] = pdata[5];
	data[7] = center.y - data[3] * center.x - data[4] * center.y - data[5] * center.z + offset.y;
	data[8] = pdata[6], data[9] = pdata[7], data[10] = pdata[8];
	data[11] = center.z - data[6] * center.x - data[7] * center.y - data[8] * center.z + offset.z;
}

template<typename T, typename S>
inline void getRotationMatrix23(T* data, Point_<S> center, double angle, double scale)
{
	return getRotationMatrix23<T, S>(data, center, angle, scale, center.x, center.y);
}

Point2f transPoint23f(const Point2f& p, float* rotMat);
void transPoint23f(vector<Point2f>& pnts, float* rotMat);
void transPoint23f(vector<Point3f>& pnts, float* rotMat);

template<typename T, typename S>
inline void transPoint23(S& x, S& y, const T* rotMat)
{
	S a = saturate_cast<S>(rotMat[0] * x + rotMat[1] * y + rotMat[2]);
	y = saturate_cast<S>(rotMat[3] * x + rotMat[4] * y + rotMat[5]);
	x = a;
}

template<typename T>
inline void transPoint23(Vec4f& line, const T* rotMat)
{
	transPoint23(line[0], line[1], rotMat);
	transPoint23(line[2], line[3], rotMat);
}

template<typename T, typename S>
inline void transPoint23(vector<Point_<S> >& pnts, T* rotMat)
{
	size_t pcount = pnts.size();
	for (size_t i = 0; i < pcount; ++i) {
		Point_<S>& p = pnts[i];
		transPoint23(p.x, p.y, rotMat);
	}
}

template<typename T, typename S>
void transPoint23(vector<Point3_<S> >& pnts, T* rotMat)
{
	size_t pcount = pnts.size();
	for (size_t i = 0; i < pcount; ++i) {
		Point3_<S>& p = pnts[i];
		float x = saturate_cast<S>(rotMat[0] * p.x + rotMat[1] * p.y + rotMat[2]);
		float y = saturate_cast<S>(rotMat[3] * p.x + rotMat[4] * p.y + rotMat[5]);
		p.x = x, p.y = y;
	}
}

template<typename T>
void transRotateRect23(RotatedRect& rr, T* rotMat)
{
	float r = degreeToRadian(rr.angle);
	Point2f angleDir = rr.center + Point2f(cos(r), sin(r));
	transPoint23(rr.center.x, rr.center.y, rotMat);
	transPoint23(angleDir.x, angleDir.y, rotMat);
	rr.angle = radianToDegree(atan2(angleDir.y - rr.center.y, angleDir.x - rr.center.x));
}

Matx34f getRotationMatrix34f(Point3f center, Point3f angle, Point3f offset);

Point3f transPoint34f(const Point3f& p, float* rotMat);
void transPoint34f(vector<Point3f>& pnts, float* rotMat);

template<typename T, typename S>
inline Point3_<S> transPoint34(const Point3_<S>& p, T* rotMat)
{
	return Point3_<S>(saturate_cast<S>(rotMat[0] * p.x + rotMat[1] * p.y + rotMat[2] * p.z + rotMat[3]),
		saturate_cast<S>(rotMat[4] * p.x + rotMat[5] * p.y + rotMat[6] * p.z + rotMat[7]),
		saturate_cast<S>(rotMat[8] * p.x + rotMat[9] * p.y + rotMat[10] * p.z + rotMat[11]));
}

template<typename T, typename S>
void transPoint34(vector<Point3_<S>>& pnts, T* rotMat)
{
	size_t pcount = pnts.size();
	for (size_t i = 0; i < pcount; ++i) {
		pnts[i] = transPoint34(pnts[i], rotMat);
	}
}

template<typename T, typename S>
void addScalePoints(vector<Point_<T> >& pnts, const Point_<S>& scale, const Point_<S>& shift) {
	size_t c = pnts.size();
	for (size_t i = 0; i < c; ++i) {
		Point_<T>& p = pnts[i];
		p.x = p.x * scale.x + shift.x;
		p.y = p.y * scale.y + shift.y;
	}
}

template<typename T, typename S>
void addPoints(vector<Point_<T> >& pnts, const Point_<S>& shift) {
	size_t c = pnts.size();
	for (size_t i = 0; i < c; ++i) {
		Point_<T>& p = pnts[i];
		p.x = p.x + shift.x;
		p.y = p.y + shift.y;
	}
}

template<typename T, typename S>
void scalePoints(vector<Point_<T> >& pnts, const Point_<S>& scale) {
	size_t c = pnts.size();
	for (size_t i = 0; i < c; ++i) {
		Point_<T>& p = pnts[i];
		p.x = p.x * scale.x;
		p.y = p.y * scale.y;
	}
}

template<typename T, typename S>
void addScalePoints(vector<Point3_<T> >& pnts, const Point3_<S>& scale, const Point3_<S>& shift) {
	size_t c = pnts.size();
	for (size_t i = 0; i < c; ++i) {
		Point3_<T>& p = pnts[i];
		p.x = p.x * scale.x + shift.x;
		p.y = p.y * scale.y + shift.y;
		p.z = p.z * scale.z + shift.z;
	}
}

template<typename T, typename S>
void addPoints(vector<Point3_<T> >& pnts, const Point3_<S>& shift) {
	size_t c = pnts.size();
	for (size_t i = 0; i < c; ++i) {
		Point3_<T>& p = pnts[i];
		p.x = p.x + shift.x;
		p.y = p.y + shift.y;
		p.z = p.z + shift.z;
	}
}

template<typename T, typename S>
void scalePoints(vector<Point3_<T> >& pnts, const Point3_<S>& scale) {
	size_t c = pnts.size();
	for (size_t i = 0; i < c; ++i) {
		Point3_<T>& p = pnts[i];
		p.x = p.x * scale.x;
		p.y = p.y * scale.y;
		p.z = p.z * scale.z;
	}
}

void getRigidTransform(const Point2f& cen1, const Point2f& u1, const Point2f& u2,
	const Point2f& cen2, const Point2f& v1, const Point2f& v2,
	double& x0, double& y0, double& angle, double& scale);

void getRigidTransform(const Point2f& u1, const Point2f& u2, const Point2f& v1, const Point2f& v2,
	double& x0, double& y0, double& angle, double& scale);

void getRigidTransform(const Point2f& cen1, const Point2f& u1, float ua,
	const Point2f& cen2, const Point2f& v1, float va,
	double& x0, double& y0, double& angle, double& scale);

void getRigidTransform(const Point2f& u1, float ua, const Point2f& v1, float va,
	double& x0, double& y0, double& angle, double& scale);

void getRigidTransform(const Point2f& cen1, const Point2f& u1, float ua,
	const Point2f& cen2, const Point2f& v1, float va, float s,
	double& x0, double& y0, double& angle, double& scale);

void getRigidTransform(const Point2f& u1, float ua, const Point2f& v1, float va, float s,
	double& x0, double& y0, double& angle, double& scale);

CYCLOPS_GEOM_DLLSPEC Mat applyPerspectiveTransform(const Mat& img,
	std::vector<Point2f>& transVertexes, // in order top-left, top-right, bottom-right, bottom-left
	int flags = INTER_LINEAR, int borderMode = BORDER_CONSTANT,
	const Scalar& borderValue = Scalar());

template<typename _Ty>
inline Mat transVecToMat3D(const Mat& tVec)
{
	_ASSERTE(tVec.total() == 3);

	Mat tMat = Mat::zeros(3, 4, CV_MAKETYPE(DataType<_Ty>::type, 1));
	_Ty* tVecData = (_Ty*)(tVec.data);
	tMat.at<_Ty>(0, 0) = 1.0;
	tMat.at<_Ty>(1, 1) = 1.0;
	tMat.at<_Ty>(2, 2) = 1.0;
	tMat.at<_Ty>(0, 3) = tVecData[0];
	tMat.at<_Ty>(1, 3) = tVecData[1];
	tMat.at<_Ty>(2, 3) = tVecData[2];

	return tMat;
}

template<typename T, typename S>
inline void applyHomography(S& x, S& y, const T* homoMat)
{
	double z = x * homoMat[6] + y * homoMat[7] + homoMat[8];
	S x0 = saturate_cast<S>((x * homoMat[0] + y * homoMat[1] + homoMat[2]) / z);
	y = saturate_cast<S>((x * homoMat[3] + y * homoMat[4] + homoMat[5]) / z);
	x = x0;
}

template<typename T, typename S>
void applyHomography(Point_<S>& pnt, T* homoMat) {
	applyHomography<T, S>(pnt.x, pnt.y, homoMat);
}

enum EnumPntsMappingType {
	PntMapTypeAffine = 0,
	PntMapTypeRigid,
	PntMapTypeHomo,
};
CYCLOPS_GEOM_DLLSPEC Mat findPntsMapping(vector<Point2d>& src, vector<Point2d>& dst, EnumPntsMappingType type);

/************************************************************************/
/*                               Shape                                  */
/************************************************************************/

CYCLOPS_GEOM_DLLSPEC float isParallel(const Vec4f& l1, const Vec4f& l2, float err_tol = 1e-9);

// returns area/avg(l1.length + l2.length), smaller value if the two lines are more likely collinear
CYCLOPS_GEOM_DLLSPEC float isCollinear(const Vec4f& l1, const Vec4f& l2, float err_tol = 1e-9);

// return cosine of the biggest shape angle, bigger value if the three points are more likely collinear
CYCLOPS_GEOM_DLLSPEC float isCollinear(const Point2f& p1, const Point2f& p2, const Point2f& p3, float err_tol = 1e-9);

// returns (rmin/rmax), [0, 1], bigger value if the two circles are more likely concentric.
// return 0 if they are intersected or no overlap.
CYCLOPS_GEOM_DLLSPEC float isConcentric(const Vec3f& c1, const Vec3f& c2, float err_tol = 1e-9);

bool isLikeArch(const Mat& vec, double tor);

bool isLikeHorizontalLine(const Mat& vec, double tor);

double isLikeLine(const vector<Point2f>& pnts, int distType, Vec4f& line);

bool isPntInCircle(const Vec3f& circle, const Point2f& p);
Vec3f makeCircle(const Point2f& p1, const Point2f& p2);
Vec3f makeCircle(const Point2f& p1, const Point2f& p2, const Point2f& p3);
CYCLOPS_GEOM_DLLSPEC void minAreaCircle(const vector<Point2f>& pnts, Vec3f& circle);

CYCLOPS_GEOM_DLLSPEC RotatedRect minAreaRect2(InputArray pnts);

bool approxMinAreaQuadix(const vector<Point2f>& pnts, vector<Point2f>& corners, bool isClockWise = false);
bool approxMinAreaQuadix(const vector<Point>& pnts, vector<Point>& corners, bool isClockWise = false);

CYCLOPS_GEOM_DLLSPEC bool approxMinAreaPoly(const vector<Point2f>& pnts, vector<Point2f>& corners, bool isClockWise = false, int maxVertex = 0);
bool approxMinAreaPoly(const vector<Point>& pnts, vector<Point>& corners, bool isClockWise = false, int maxVertex = 0);

void approxPolyVSV(const vector<Point2f>& pnts, vector<Point2f>& corners, int number, bool closed);
void approxPolyVSV(const vector<Point>& pnts, vector<Point>& corners, int number, bool closed);

vector<Point2f> reorderToAlignPnt(const vector<Point2f>& pnts, const Point2f& alignPnt);
vector<Point> reorderToAlignPnt(const vector<Point>& pnts, const Point& alignPnt);

template<typename T>
inline T getTriangleArea(T ax, T ay, T bx, T by, T cx, T cy)
{
	double area = ((bx - ax)*(cy - ay) - (cx - ax)*(by - ay)) / 2.;
	return saturate_cast<T>(abs(area));
}

template<typename T>
inline T getTriangleArea(const Point_<T>& a, const Point_<T>& b, const Point_<T>& c)
{
	return getTriangleArea(a.x, a.y, b.x, b.y, c.x, c.y);
}

template<typename T>
inline float getConvexHullArea(const vector<Point_<T> >& contour)
{
	vector<Point_<T> > hull;
	convexHull(contour, hull);
	return contourArea(hull);
}

CYCLOPS_GEOM_DLLSPEC float getInertia(const Moments& moms);

float getOrientation(const Moments& moms);

template<typename T>
inline bool circumcenter(T x1, T y1, T x2, T y2, T x3, T y3, T& px, T& py)
{
	T a = x2 - x1;
	T b = y2 - y1;
	T c = x3 - x1;
	T d = y3 - y1;
	T e = a * (x1 + x2) + b * (y1 + y2);
	T f = c * (x1 + x3) + d * (y1 + y3);
	T g = T(2.0) * (a * (y3 - y2) - b * (x3 - x2));

	if (abs(g) < FLT_EPSILON) {
		return false; // it means the 3 points may be collinear
	}
	else {
		px = (d * e - b * f) / g;
		py = (a * f - c * e) / g;
		return true;
	}
}

template<typename T>
inline bool circumcenter(const Point_<T>& p1, const Point_<T>& p2, const Point_<T>& p3,
	Point_<T>& p)
{
	return circumcenter(p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, p.x, p.y);
}

Vec4f midLine(const Vec4f& l1, const Vec4f& l2);

//*****************by reference equation from web*************
//http://www.ambrsoft.com/TrigoCalc/Circles2/Circles2Tangent_.htm
//the func generates two pairs of outer tangent over two circles
//each pair could be formed as line which is tangent line on the
//one side of two circles
CYCLOPS_GEOM_DLLSPEC void outerTangentLine2Circle(const Vec3d& circle1,
	const Vec3d& circle2,
	std::pair<Point2d, Point2d>& pp1,
	std::pair<Point2d, Point2d>& pp2);

/***************** reference paper *************/
//"poles of Inaccessibility:A calculation algorithm for the 
//remotest places on earth"
CYCLOPS_GEOM_DLLSPEC void getInscribedCircleOnPolygonBySequency(const vector<Point2f>& poly,
	Vec3f & circle);

// count pixels inside the polygon contour, works only for contour of dense points (out of CHAIN_APPROX_NONE)
int countPixelInPolygon(const vector<Point>& poly, bool countBorder = true);

};

using namespace GeomUtils;

/************************************************************************/
/*             Equal, Less, Hash injection to STD                       */
/************************************************************************/

struct RectHasher
{
	size_t operator()(Rect const& r) const noexcept
	{
		return std::hash<int>{}(r.x) ^ (std::hash<int>{}(r.y) << 1) ^
			(std::hash<int>{}(r.width) << 2) ^ (std::hash<int>{}(r.height) << 3);
	}
};

struct RectEqualFunc
{
	bool operator() (const Rect& r1, const Rect& r2) const {
		return r1.x == r2.x && r1.y == r2.y &&
			r1.width == r2.width && r1.height == r2.height;
	}
};

#endif // GeomUtils_h_