You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
kev/Drawer/GVision/BaseLib/PublicFunctions.cpp

497 lines
10 KiB
C++

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

#include "stdafx.h"
#include "PublicFunctions.h"
#include "Clipper/clipperhandler.h"
#include <sstream>
#include <list>
#ifndef ZERO6
#define ZERO6 1e-6
#endif
#ifndef ZERO
#define ZERO 1e-8
#endif
using namespace NClipperLib;
using namespace std;
//#ifndef RHO
//#define RHO 57.295779513082320876798154814105
//#endif
CPublicFunctions::CPublicFunctions()
{
}
CPublicFunctions::~CPublicFunctions()
{
}
void CPublicFunctions::GetGravityCenter(double* xx, double* yy, int num, double& x0, double& y0)
{
if (num < 2)
return;
if (num == 2)
{
x0 = (xx[0] + xx[1]) / 2.0;
y0 = (yy[0] + yy[1]) / 2.0;
return;
}
double x[3], y[3];
memcpy(x, xx, sizeof(double) * 3);
memcpy(y, yy, sizeof(double) * 3);
double sum_x = 0;
double sum_y = 0;
double sum_area = 0;
double area = 0;
for (int i = 2; i < num; i++)
{
x[1] = xx[i - 1];
y[1] = yy[i - 1];
x[2] = xx[i];
y[2] = yy[i];
area = GetAreaTrue(3, x, y);
sum_area += area;
sum_x += (x[0] + x[1] + x[2])*area;
sum_y += (y[0] + y[1] + y[2])*area;
}
x0 = sum_x / sum_area / 3.0;
y0 = sum_y / sum_area / 3.0;
}
double CPublicFunctions::GetAreaTrue(int num, double* x, double* y)
{
int i;
double s;
s = (y[0] + y[num - 1]) * (x[0] - x[num - 1]);
for (i = 0; i < num - 1; ++i)
s += (y[i + 1] + y[i]) * (x[i + 1] - x[i]);
return s * 0.5;
}
double CPublicFunctions::LinearInterpolate(double x1, double y1, double x2, double y2, double x0)
{
if (fabs(x1 - x2) < ZERO6)
return (y1 + y2) / 2.0;
if (fabs(x1 - x0) < ZERO6)
return y1;
if (fabs(x2 - x0) < ZERO6)
return y2;
double y0 = 0;
double k = (x0 - x1) / (x2 - x1);
y0 = y1 + k*(y2 - y1);
return y0;
}
void CPublicFunctions::Trim(string& line)
{
if (line.size() < 1)
return;
size_t idx1 = line.find_first_not_of(' ');
size_t idx2 = line.find_last_not_of(' ');
if (idx1 > idx2)
{
line.clear();
return;
}
line = line.substr(idx1, idx2 - idx1 + 1);
}
double CPublicFunctions::Area(int num, double * x, double * y)
{
int i;
double s;
s = (y[0] + y[num - 1]) * (x[0] - x[num - 1]);
for (i = 0; i < num - 1; ++i)
s += (y[i + 1] + y[i]) * (x[i + 1] - x[i]);
return fabs(s * 0.5);
}
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>(x1,y1)-(x2,y2) ,(x3,y3)-(x4,y4)<29>Ƿ<EFBFBD><C7B7>
int CPublicFunctions::IsSegmentCross(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
{
//<2F><><EFBFBD>ο<EFBFBD><CEBF><EFBFBD><EFBFBD>ų<EFBFBD>ʵ<EFBFBD><CAB5>
double xmin1 = min(x1, x2);
double ymin1 = min(y1, y2);
double xmax1 = max(x1, x2);
double ymax1 = max(y1, y2);
double xmin2 = min(x3, x4);
double ymin2 = min(y3, y4);
double xmax2 = max(x3, x4);
double ymax2 = max(y3, y4);
if (xmin1 > xmax2 + ZERO6 || xmax1 + ZERO6 < xmin2)
return 0;
if (ymin1 > ymax2 + ZERO6 || ymin2 > ymax1 + ZERO6)
return 0;
//<2F><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>
double x12 = x2 - x1; //<2F><><EFBFBD><EFBFBD>
double y12 = y2 - y1;
double x23 = x3 - x2;
double y23 = y3 - y2;
double x34 = x4 - x3;
double y34 = y4 - y3;
double x13 = x3 - x1;
double y13 = y3 - y1;
double x14 = x4 - x1;
double y14 = y4 - y1;
//<2F>ж<EFBFBD>3,4<><34><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>1-2<><32><EFBFBD><EFBFBD>
if (CrossProduct(x12, y12, x13, y13) * CrossProduct(x12, y12, x14, y14) > 0)
return 0; //<2F><>3,4<><34> 1-2<>߶<EFBFBD>ͬ<EFBFBD><CDAC>
if (CrossProduct(x34, y34, x13, y13) * CrossProduct(x34, y34, x23, y23) > 0)
return 0; //<2F><>1,2<><32> 3-4<>߶<EFBFBD>ͬ<EFBFBD><CDAC>
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
if (fabs(CrossProduct(x12, y12, x34, y34)) < 1e-8)
return 2;
return 1;
}
bool CPublicFunctions::IsEqual(double x, double y, double esp)
{
if (fabs(x - y) < esp)
return true;
return false;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A(x1,y1),B(x2,y2)<29>IJ<EFBFBD><C4B2><EFBFBD>(<28><><EFBFBD><EFBFBD>)
double CPublicFunctions::CrossProduct(double x1, double y1, double x2, double y2)
{
return (x1*y2 - x2*y1);
}
double CPublicFunctions::CrossProduct(double x1, double y1, double x2, double y2, double ptX, double ptY)
{
return (x2 - x1) * (ptY - y1) - (y2 - y1) * (ptX - x1);
}
int CPublicFunctions::IsInsidePolygon(double x0, double y0, double * x, double * y, int m)
{
if (m < 3)
return 0;
double x1 = x0;
double y1 = y0;
double x2 = x0 - 1e10;
double y2 = y0;
const double zero8 = 1e-8;
int crossNum = 0;
int iprev;
int inext;
for (int i = 0; i < m - 1; i++)
{
if (fabs(x[i] - x[i + 1]) < zero8 && fabs(y[i] - y[i + 1]) < 1e-8)
continue;
if (fabs(y[i + 1] - y0) < zero8 && x[i + 1] < x0)
continue;
if (fabs(y[i] - y0) < zero8 && x[i] < x0)
{
if (0 == i)
{
iprev = m - 1;
while (IsEqual(x[iprev], x[i], zero8) && IsEqual(y[iprev], y[i], zero8))
iprev--;
}
else
iprev = i - 1;
inext = i + 1;
if (((y0 - y[iprev]) > -1e-8 && (y[inext] - y0) > -1e-8) ||
(((y0 - y[iprev]) < -1e-8 && (y[inext] - y0)< -1e-8))
)
crossNum++;
}
else if (IsSegmentCross(x1, y1, x2, y2, x[i], y[i], x[i + 1], y[i + 1]))
crossNum++;
}
//ĩβ<C4A9><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (fabs(x[0] - x[m - 1]) > zero8 || fabs(y[0] - y[m - 1]) > zero8)
{
if (fabs(y[0] - y0) > zero8 || x[0] > x0)
{
if (fabs(y[m - 1] - y0) < zero8 && x[m - 1] < x0)
{
int i = m - 1;
if (((y0 - y[i - 1]) > -1e-8 && (y[0] - y0) > -1e-8) ||
(((y0 - y[i - 1]) < -1e-8 && (y[0] - y0)< -1e-8))
)
crossNum++;
}
else if (IsSegmentCross(x1, y1, x2, y2, x[0], y[0], x[m - 1], y[m - 1]))
crossNum++;
}
}
if (crossNum % 2 == 0) //<2F>ⲿ
return 0;
return 1;
}
double CPublicFunctions::Distance(double x1, double y1, double x2, double y2)
{
double dist = sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1));
return dist;
}
int CPublicFunctions::LineCrossPoint(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double& x0, double& y0)
{
double k1 = 1e11;
double k2 = 1e11;
if (fabs(x2-x1) > ZERO)
k1 = (y2 - y1) / (x2 - x1);
if (fabs(x4 - x3) > ZERO)
k2 = (y4 - y3) / (x4 - x3);
return LineCrossPoint(x1, y1, k1, x3, y3, k2, x0, y0);
}
//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD>ߵĽ<DFB5><C4BD><EFBFBD>,k1,k2Ϊ<32><CEAA><EFBFBD><EFBFBD>ֱ<EFBFBD>ߵ<EFBFBD>б<EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD>kֵ > 1e10 <20><>ʾ<EFBFBD><CABE>ֱ<EFBFBD><D6B1>x<EFBFBD><78> <20><><EFBFBD><EFBFBD>0<EFBFBD><30>ʾ<EFBFBD>޽<EFBFBD><DEBD><EFBFBD><E3A3AC><EFBFBD><EFBFBD>1<EFBFBD><31>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>Ψһ<CEA8><D2BB><EFBFBD><EFBFBD><E3A3AC><EFBFBD><EFBFBD>2<EFBFBD><32>ʾ<EFBFBD><CABE>ֱ<EFBFBD><D6B1><EFBFBD>غ<EFBFBD>
int CPublicFunctions::LineCrossPoint(double x1, double y1, double k1, double x2, double y2, double k2, double& x0, double& y0)
{
if (fabs(k1) > 1e10 && fabs(k2) > 1e10) //ͬʱ<CDAC><CAB1>ֱ<EFBFBD><D6B1>x<EFBFBD><78>
{
if (fabs(x2 - x1) < ZERO6) //<2F><><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD>
{
x0 = x1;
y0 = y1;
return 2;
}
return 0;
}
//<2F><>ƽ<EFBFBD><C6BD>
if (fabs(k1 - k2) < ZERO)
{
//<2F><><EFBFBD><EFBFBD>x<EFBFBD><78><EFBFBD><EFBFBD>
if (fabs(x2 - x1) < ZERO)
return 0;
double k = (y2 - y1) / (x2 - x1);
if (fabs(k1 - k) < 1e-10) // б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
{
x0 = x1;
y0 = y1;
return 2;
}
return 0;
}
//<2F><>б<EFBFBD>ʲ<EFBFBD><CAB2><EFBFBD>
if (fabs(x2 - x1) < ZERO && fabs(y2 - y1) < ZERO)
{
x0 = x1;
y0 = y1;
return 1;
}
//<2F><>1<EFBFBD><31>ֱx<D6B1><78>
if (fabs(k1) > 1e10)
{
if (fabs(x2 - x1) < ZERO)
{
x0 = x2;
y0 = y2;
return 1;
}
x0 = x1;
y0 = y2 - k2*(x2 - x0);
return 1;
}
//<2F><>2<EFBFBD><32>ֱx<D6B1><78>
if (fabs(k2) > 1e10)
{
if (fabs(x2 - x1) < ZERO)
{
x0 = x1;
y0 = y1;
return 1;
}
x0 = x2;
y0 = y1 - k1*(x1 - x0);
return 1;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>x<EFBFBD><78>
x0 = -(y1 - y2 - k1*x1 + k2*x2) / (k1 - k2);
y0 = y1 - k1*(x1 - x0);
return 1;
}
//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD> -pi/2 ~pi/2
double CPublicFunctions::GetAngle180(double x1, double y1, double x2, double y2)
{
if (fabs(x1 - x2) < ZERO)
return PI / 2;
return atan((y2 - y1) / (x2 - x1));
}
//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD> -pi ~ pi
double CPublicFunctions::GetAngle360(double x1, double y1, double x2, double y2)
{
return atan2(y2 - y1, x2 - x1);
}
void CPublicFunctions::Rotate(double &x0, double &y0, double radians, int directionFlag)
{
Rotate(x0, y0, cos(radians), sin(radians), directionFlag);
//double xp,yp;
//xp=x0;
//yp=y0;
//x0=yp*sin(angle)+xp*cos(angle);
//y0=yp*cos(angle)-xp*sin(angle);
}
void CPublicFunctions::Rotate(double &x, double &y, double cosa, double sina, int directionFlag)
{
if (directionFlag == 1)
{
double tx, ty;
tx = x * cosa + y * sina;
ty = y * cosa - x * sina;
x = tx;
y = ty;
}
// <20>ϲ<EFBFBD><CFB2><EFBFBD>ע<EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD>
else {
double tx, ty;
tx = x * cosa - y * sina;
ty = x * sina + y * cosa;
x = tx;
y = ty;
}
}
void CPublicFunctions::Rotate(double& x0, double& y0, double xs, double ys, double radian)
{
double tx = x0 - xs;
double ty = y0 - ys;
Rotate(tx, ty, radian);
x0 = tx + xs;
y0 = ty + ys;
}
void CPublicFunctions::Rotate(double& x0, double& y0, double xs, double ys,
double cosa, double sina)
{
double tx = x0 - xs;
double ty = y0 - ys;
Rotate(tx, ty, cosa,sina);
x0 = tx + xs;
y0 = ty + ys;
}
// <20>жϵ<D0B6><CFB5><EFBFBD>ֱ<EFBFBD>ߵ<EFBFBD><DFB5>IJ<EFBFBD> 1-- <20><><EFBFBD><EFBFBD> 0--<2D><><EFBFBD><EFBFBD> 2---<2D>Ҳ<EFBFBD>
/////////////////////////////////////////////////////////////////////////
int CPublicFunctions::PointInLine(double x0, double y0, double x1, double y1, double x2, double y2)
{
double r = (x1 - x0) * (y2 - y0) - (y1 - y0) * (x2 - x0);
if (fabs(r) < ZERO6)
return 0;
else if (r > 0.0)
return 1;
else
return 2;
}
bool CPublicFunctions::IsPointOnsegment(double x0, double y0, double x1, double y1, double x2, double y2)
{
if (x0 < min(x1, x2) - ZERO6)
return false;
if (x0 > max(x1, x2) + ZERO6)
return false;
if (y0 < min(y1, y2) - ZERO6)
return false;
if (y0 > max(y1, y2) + ZERO6)
return false;
if (0 == PointInLine(x0, y0, x1, y1, x2, y2))
return true;
return false;
}
double CPublicFunctions::PointLineDistance(double x0, double y0, double k, double b)
{
if (fabs(k) > 1e30)
return x0 - b;
return (y0 - k * x0 - b) / sqrt(1.0 + k * k);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>֮<EFBFBD><D6AE><EFBFBD>ļн<C4BC>
double CPublicFunctions::GetAngle(double x1, double y1, double x2, double y2, double x3, double y3, int directionFlag)
{
double RHO = 57.295779513082320876798154814105;
double tx, ty;
tx = x2 - x1;
ty = y2 - y1;
double angle = 90;
if (fabs(tx) < 1e-10) //<2F><>ֹ<EFBFBD><D6B9>0<EFBFBD><30>
{
if (ty > 1e-10)angle = 90.0;
else angle = -90.0;
}
else
angle = atan2(ty, tx);
tx = x3 - x2;
ty = y3 - y2;
Rotate(tx, ty, angle, directionFlag);
if (fabs(tx) < 1e-10)
{
if (ty > 1e-10) return 90.0;
else return -90.0;
}
return atan2(ty, tx) * RHO;
}
int CPublicFunctions::BinarySearch(double key, std::vector<double>& x)
{
int i1, i2, i;
int num = (int)x.size();
i1 = 0; i2 = num - 1;
if (x[i2] - x[i1] > 1e-10)
{
if (key > x[i2])return num + 100;
if (key < x[i1])return -100;
}
else if (x[i2] < x[i1])
{
if (key < x[i2])return num + 100;
if (key > x[i1])return -100;
}
if (fabs(key - x[i1]) < 1e-10)return 0;
if (fabs(key - x[i2]) < 1e-10)return i2 - 1;
while (i2 - i1 > 1)
{
if (fabs(x[i2] - x[i1]) < 1e-10)break;
i = (i1 + i2) / 2;
if ((key - x[i]) * (key - x[i2]) > 1e-10)
i2 = i;
else i1 = i;
}
return i1;
}