BaseLibraryCPP/BaseTool.cpp

#include "stdafx.h"
#define EIGEN_USE_BLAS
#define EIGEN_VECTORIZE_SSE4_2
//#include <mkl.h>

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
////#include <mkl.h>
 
#include <omp.h>
#include <QDir>
#include < io.h >
#include < stdio.h >
#include < stdlib.h >
#include <gdal.h>
#include <gdal_priv.h>
#include <gdalwarper.h>
//#include <ogr_geos.h>
#include <ogrsf_frmts.h> //#include "ogrsf_frmts.h"

#include <fstream>
#include <proj.h>
#include "GeoOperator.h"

#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>
#include "baseTool.h"

#include <gsl/gsl_fit.h> // 多项式拟合
#include <gsl/gsl_cdf.h>    /* 提供了 gammaq 函数 */
#include <gsl/gsl_vector.h> /* 提供了向量结构*/
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
#include <qcoreapplication.h>





QString   getCurrentTimeString() {
	struct tm ConversionTime;
	std::time_t t = std::time(NULL);
	char mbstr[100];
	_localtime64_s(&ConversionTime, &t);
	std::strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %H:%M:%S", &ConversionTime);
	QString strTime = mbstr;
	return strTime;
}

QString   getCurrentShortTimeString() {
	struct tm ConversionTime;
	std::time_t t = std::time(NULL);
	char mbstr[100];
	_localtime64_s(&ConversionTime, &t);
	std::strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %H:%M:%S", &ConversionTime);
	QString strTime = mbstr;
	return strTime;
}

std::vector<QString>   splitString(const QString& str, char delimiter)
{
	QStringList tokens = str.split(delimiter);
	return convertQStringListToStdVector(tokens);
}


std::complex<double>   Cubic_Convolution_interpolation(double u, double v, Eigen::MatrixX<std::complex<double>> img)
{
	if (img.rows() != 4 || img.cols() != 4) {
		throw std::exception("the size of img's block is not right");
	}
	// 斤拷锟斤拷模锟斤拷
	Eigen::MatrixX<std::complex<double>> wrc(1, 4);// 使锟斤拷 complex<double> 斤拷锟斤拷要原斤拷为锟剿伙拷取值
	Eigen::MatrixX<std::complex<double>> wcr(4, 1);//
	for (int i = 0; i < 4; i++) {
		wrc(0, i) = Cubic_kernel_weight(u + 1 - i); // u+1,u,u-1,u-2
		wcr(i, 0) = Cubic_kernel_weight(v + 1 - i);
	}

	Eigen::MatrixX<std::complex<double>> interValue = wrc * img * wcr;
	return interValue(0, 0);
}

std::complex<double>   Cubic_kernel_weight(double s)
{
	s = abs(s);
	if (s <= 1) {
		return std::complex<double>(1.5 * s * s * s - 2.5 * s * s + 1, 0);
	}
	else if (s <= 2) {
		return std::complex<double>(-0.5 * s * s * s + 2.5 * s * s - 4 * s + 2, 0);
	}
	else {
		return std::complex<double>(0, 0);
	}
}

/// <summary>
///  p11  p12 -- x 
///    p0(u,v)
///  p21  p22
///   |
///   y
/// p11(0,0)
/// p21(0,1)
/// P12(1,0)
/// p22(1,1)
/// </summary>
/// <param name="p0">x,y,z</param>
/// <param name="p1">x,y,z</param>
/// <param name="p2">x,y,z</param>
/// <param name="p3">x,y,z</param>
/// <param name="p4">x,y,z</param>
/// <returns></returns>
double   Bilinear_interpolation(Landpoint p0, Landpoint p11, Landpoint p21, Landpoint p12, Landpoint p22)
{
	return	p11.ati * (1 - p0.lon) * (1 - p0.lat) +
		p12.ati * p0.lon * (1 - p0.lat) +
		p21.ati * (1 - p0.lon) * p0.lat +
		p22.ati * p0.lon * p0.lat;
}



bool   onSegment(Point3 Pi, Point3 Pj, Point3 Q)
{
	if ((Q.x - Pi.x) * (Pj.y - Pi.y) == (Pj.x - Pi.x) * (Q.y - Pi.y)  //叉乘 
		//保证Q点坐标在pi,pj之间 
		&& std::min(Pi.x, Pj.x) <= Q.x && Q.x <= std::max(Pi.x, Pj.x)
		&& std::min(Pi.y, Pj.y) <= Q.y && Q.y <= std::max(Pi.y, Pj.y))
		return true;
	else
		return false;
}

Point3   invBilinear(Point3 p, Point3 a, Point3 b, Point3 c, Point3 d)
{
	Point3 res;

	Point3 e = b - a;
	Point3 f = d - a;
	Point3 g = a - b + c - d;
	Point3 h = p - a;

	double k2 = cross2d(g, f);
	double k1 = cross2d(e, f) + cross2d(h, g);
	double k0 = cross2d(h, e);
	double u, v;
	// if edges are parallel, this is a linear equation
	if (abs(k2) < 0.001)
	{
		v = -k0 / k1;
		u = (h.x - f.x * v) / (e.x + g.x * v);
		p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v;
		return p;
	}
	// otherwise, it's a quadratic
	else
	{
		float w = k1 * k1 - 4.0 * k0 * k2;
		if (w < 0.0) {
			// 可能在边界上
			if (onSegment(a, b, p)) {
				Point3 tt = b - a;
				Point3 ttpa = p - a;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = a.z + scater * tt.z;
				return p;
			}
			else if (onSegment(b, c, p)) {
				Point3 tt = c - b;
				Point3 ttpa = p - b;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = b.z + scater * tt.z;
				return p;
			}
			else if (onSegment(c, d, p)) {
				Point3 tt = d - c;
				Point3 ttpa = p - c;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = c.z + scater * tt.z;
				return p;
			}
			else if (onSegment(d, a, p)) {
				Point3 tt = a - d;
				Point3 ttpa = p - d;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = d.z + scater * tt.z;
				return p;
			}

			return { -9999,-9999,-9999 };
		}
		else {
			w = sqrt(w);

			float ik2 = 0.5 / k2;
			float v = (-k1 - w) * ik2;
			float u = (h.x - f.x * v) / (e.x + g.x * v);

			if (u < 0.0 || u>1.0 || v < 0.0 || v>1.0)
			{
				v = (-k1 + w) * ik2;
				u = (h.x - f.x * v) / (e.x + g.x * v);
			}
			p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v;
			return p;
		}
	}
	p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v;
	return p;
}

double   sind(double degree)
{
	return sin(degree * d2r);
}

double   cosd(double d)
{
	return cos(d * d2r);
}


std::string   Convert(float Num)
{
	std::ostringstream oss;
	oss << Num;
	std::string str(oss.str());
	return str;
}

QString   JoinPath(const QString& path, const QString& filename)
{
	QDir dir(path);

	// Ensure the path ends with the appropriate separator
	if (!QDir::isAbsolutePath(path))
		dir.makeAbsolute();

	return dir.filePath(filename);
}
std::vector<QString>   convertQStringListToStdVector(const QStringList& qStringList)
{
	std::vector<QString> stdVector;

	for (const QString& str : qStringList) {
		stdVector.push_back(str);
	}

	return stdVector;
};


ErrorCode polyfit(const double* x, const double* y, int xyLength, int poly_n, std::vector<double>& out_factor, double& out_chisq) {
	/*
	 * x：自变量，视差
	 * y：因变量，距离
	 * xyLength: x、y长度
	 * poly_n：拟合的阶次
	 * out_factor：拟合的系数结果，从0阶到poly_n阶的系数
	 * out_chisq：拟合曲线与数据点的优值函数最小值 ,χ2 检验
	*/

	gsl_matrix* XX = gsl_matrix_alloc(xyLength, poly_n + 1);
	gsl_vector* c = gsl_vector_alloc(poly_n + 1);
	gsl_matrix* cov = gsl_matrix_alloc(poly_n + 1, poly_n + 1);
	gsl_vector* vY = gsl_vector_alloc(xyLength);

	for (size_t i = 0; i < xyLength; i++)
	{
		gsl_matrix_set(XX, i, 0, 1.0);
		gsl_vector_set(vY, i, y[i]);
		for (int j = 1; j <= poly_n; j++)
		{
			gsl_matrix_set(XX, i, j, pow(x[i], j));
		}
	}
	gsl_multifit_linear_workspace* workspace = gsl_multifit_linear_alloc(xyLength, poly_n + 1);
	int r = gsl_multifit_linear(XX, vY, c, cov, &out_chisq, workspace);
	gsl_multifit_linear_free(workspace);
	out_factor.resize(c->size, 0);
	for (size_t i = 0; i < c->size; ++i)
	{
		out_factor[i] = gsl_vector_get(c, i);
	}

	gsl_vector_free(vY);
	gsl_matrix_free(XX);
	gsl_matrix_free(cov);
	gsl_vector_free(c);

	return GSLState2ErrorCode(r);
}

Point3 crossProduct(const Point3& a, const Point3& b)
{
	return Point3{
	a.y * b.z - a.z * b.y, // C_x
	a.z * b.x - a.x * b.z, // C_y
	a.x * b.y - a.y * b.x  // C_z
	};
}




// 计算绕任意轴旋转的旋转矩阵
Eigen::Matrix3d rotationMatrix(const Eigen::Vector3d& axis, double angle) {
	// 确保旋转轴是单位向量
	Eigen::Vector3d u = axis.normalized();

	double cos_theta = cos(angle);
	double sin_theta = sin(angle);

	// 计算旋转矩阵
	Eigen::Matrix3d R;
	R <<
		cos_theta + u.x() * u.x() * (1 - cos_theta), u.x()* u.y()* (1 - cos_theta) - u.z() * sin_theta, u.x()* u.z()* (1 - cos_theta) + u.y() * sin_theta,
		u.y()* u.x()* (1 - cos_theta) + u.z() * sin_theta, cos_theta + u.y() * u.y() * (1 - cos_theta), u.y()* u.z()* (1 - cos_theta) - u.x() * sin_theta,
		u.z()* u.x()* (1 - cos_theta) - u.y() * sin_theta, u.z()* u.y()* (1 - cos_theta) + u.x() * sin_theta, cos_theta + u.z() * u.z() * (1 - cos_theta);

	return R;
}





long double convertToMilliseconds(const std::string& dateTimeStr) {
	// 定义日期时间字符串
	std::string dateTimeString = dateTimeStr;
	// 使用 Boost.Date_Time 解析日期时间字符串
	boost::posix_time::ptime dateTime = boost::posix_time::time_from_string(dateTimeString);

	// 将 ptime 转换为自 epoch (1970年1月1日) 以来的毫秒数
	boost::posix_time::ptime epoch(boost::gregorian::date(1970, 1, 1));
	boost::posix_time::time_duration duration = dateTime - epoch;
	long double milliseconds = duration.total_milliseconds() / 1000.0;
	return milliseconds;

}

long FindValueInStdVector(std::vector<double>& list, double& findv)
{
	if (list.size() == 0) {
		return -1;
	}
	else if (list.size() == 1) {
		if (abs(list[0] - findv) < 1e-9) {
			return 0;
		}
		else {
			return -1;
		}
	}
	else {}

	if (abs(list[0] - findv) < 1e-9) {
		return 0;
	}
	else if (abs(list[list.size() - 1] - findv) < 1e-9) {
		return list.size() - 1;
	}
	else if (list[0] > findv) {
		return -1;
	}
	else if (list[list.size() - 1] < findv) {
		return -1;
	}
	else {}



	long low = 0;
	long high = list.size() - 1;
	while (low <= high) {
		long mid = (low + high) / 2;
		if (abs(list[mid] - findv) < 1e-9) {
			return mid;
		}
		else if (list[mid] < findv) {
			low = mid + 1;
		}
		else if (list[mid] > findv) {
			high = mid - 1;
		}
		else {}
	}
	return -1;
}

long InsertValueInStdVector(std::vector<double>& list, double insertValue, bool repeatValueInsert)
{
	if (list.size() == 0) {
		list.insert(list.begin(), insertValue);
		return 0;
	}
	else if (list.size() == 1) {
		if (std::abs(list[0] - insertValue) < PRECISIONTOLERANCE) {
			if (repeatValueInsert) {
				list.push_back(insertValue);
				return 1;
			}
			else {
				return -1;
			}
		}
		else if (insertValue > list[0]) {
			list.push_back(insertValue);
			return 1;
		}
		else if (insertValue < list[0]) {
			list.push_back(insertValue);
			return 0;
		}
	}
	else {
		long low = 0;
		long high = list.size() - 1;
		long mid = -1;
		// 处理边界
		if (list[high] <= insertValue)
		{
			if (std::abs(list[high] - insertValue) < PRECISIONTOLERANCE) {
				if (repeatValueInsert) {
					list.push_back(insertValue);
				}
				else {}
			}
			else {
				list.push_back(insertValue);
			}
			return list.size() - 1;
		}

		if (list[low] >= insertValue) {

			if (std::abs(list[low] - insertValue) < PRECISIONTOLERANCE) {
				if (repeatValueInsert) {
					list.insert(list.begin(), insertValue);
				}
				else {}
			}
			else {
				list.insert(list.begin(), insertValue);
			}
			return 0;
		}
		// low<insertValue < high 
		while ((high - low) > 1) {
			mid = (low + high) / 2;
			if (std::abs(list[mid] - insertValue) < PRECISIONTOLERANCE) {
				if (repeatValueInsert) {
					list.insert(list.begin() + mid, insertValue);
				}
				return mid;
			}
			else if (insertValue < list[mid]) {
				high = mid;
			}
			else if (list[mid] < insertValue) {
				low = mid;
			}

		}
		if (list[low] <= insertValue && insertValue <= list[high])
		{

			if (std::abs(list[high] - insertValue) < PRECISIONTOLERANCE) {
				if (repeatValueInsert) {
					list.insert(list.begin() + high, insertValue);
				}
				else {}
			}
			else {
				list.insert(list.begin() + high, insertValue);
			}
			return low;
		}
		else {
			return -1;
		}
	}
	return -1;
}

long FindValueInStdVectorLast(std::vector<double>& list, double& insertValue)
{
	if (list.size() == 0 || list.size() == 1) {
		return -1;
	}
	else {
		long low = 0;
		long high = list.size() - 1;
		long mid = -1;
		// 处理边界
		if (list[high]+ PRECISIONTOLERANCE < insertValue)
		{
			return -1;
		}
		else if (std::abs(list[high] - insertValue) < PRECISIONTOLERANCE)
		{
			return high;
		}
		else if (list[low] > insertValue) {
			return -1;
		}
		else if (std::abs(list[low] - insertValue) < PRECISIONTOLERANCE) {
			return low;
		}
		else {}
		// low<insertValue < high 
		while ((high - low) > 1) {
			mid = (low + high) / 2;
			if (insertValue < list[mid]) {
				high = mid;
			}
			else if (list[mid] < insertValue) {
				low = mid;
			}
			else {// insertValue==list[mid] , list[mid]===insertValue
				return mid;// 
			}
		}
		if (list[low] <= insertValue && insertValue <= list[high])
		{
			return low;
		}
		else {
			return -1;
		}
	}
	return -1;
}


ErrorCode polynomial_fit(const std::vector<double>& x, const std::vector<double>& y, int degree, std::vector<double>& out_factor, double& out_chisq) {
	int xyLength = x.size();
	double* xdata = new double[xyLength];
	double* ydata = new double[xyLength];
	for (long i = 0; i < xyLength; i++) {
		xdata[i] = x[i];
		ydata[i] = y[i];
	}
	ErrorCode state = polyfit(xdata, ydata, xyLength, degree, out_factor, out_chisq);

	delete[] xdata;
	delete[] ydata; // 释放内存
	return state;
}

QVector<SatelliteAntPos> SatellitePos2SatelliteAntPos(QVector<SatellitePos> poses)
{
	QVector<SatelliteAntPos> antposes(poses.count());
	for (long i = 0; i < poses.count(); i++) {
		antposes[i].time = poses[i].time;
		antposes[i].Px = poses[i].Px;
		antposes[i].Py = poses[i].Py;
		antposes[i].Pz = poses[i].Pz;
		antposes[i].Vx = poses[i].Vx;
		antposes[i].Vy = poses[i].Vy;
		antposes[i].Vz = poses[i].Vz;
	}
	return antposes;
}

QVector<SatellitePos> SatelliteAntPos2SatellitePos(QVector<SatelliteAntPos> poses)
{
	QVector<SatellitePos> antposes(poses.count());
	for (long i = 0; i < poses.count(); i++) {
		antposes[i].time = poses[i].time;
		antposes[i].Px = poses[i].Px;
		antposes[i].Py = poses[i].Py;
		antposes[i].Pz = poses[i].Pz;
		antposes[i].Vx = poses[i].Vx;
		antposes[i].Vy = poses[i].Vy;
		antposes[i].Vz = poses[i].Vz;
	}
	return antposes;
}


QString getDebugDataPath(QString filename)
{
	QString folderName = "debugdata";
	QString appDir = QCoreApplication::applicationDirPath();
	QString folderpath = JoinPath(appDir, folderName);
	if (!QDir(folderpath).exists()) {
		QDir(appDir).mkdir(folderName);
	}
	QString datapath = JoinPath(folderpath, filename);
	QFile datafile(datapath);
	if (datafile.exists()) {
		datafile.remove();
	}
	return datapath;
}


std::vector<std::string> split(const std::string& str, char delimiter) {
	std::vector<std::string> tokens;
	std::string token;
	std::istringstream tokenStream(str);

	while (std::getline(tokenStream, token, delimiter)) {
		tokens.push_back(token);
	}

	return tokens;
}


Eigen::VectorXd linspace(double start, double stop, int num) {
	Eigen::VectorXd result(num);

	double step = (stop - start) / (num - 1);  // 计算步长
	for (int i = 0; i < num; ++i) {
		result[i] = start + i * step;  // 生成等间距数值
	}

	return result;
}