gch_ling
/
kev
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
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.
v4.5
main
develop
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'v4.5'
${ noResults }
kev/Drawer/GVision/BaseLib/PublicFunctions.cpp

497 lines
10 KiB
C++
Raw Permalink Normal View History Unescape Escape

搬运代码
1 month ago
#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;
}