776 lines
19 KiB
C++
776 lines
19 KiB
C++
#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>
|
||
|
||
#include <xmmintrin.h> // 包含SSE指令集头文件
|
||
#include <emmintrin.h> // 包含SSE2指令集头文件
|
||
#include <omp.h> // 包含OpenMP头文件
|
||
#include <iostream>
|
||
#include <string>
|
||
#include <boost/date_time/posix_time/posix_time.hpp>
|
||
#include <boost/date_time/posix_time/conversion.hpp>
|
||
|
||
QString longDoubleToQStringScientific(long double value) {
|
||
std::ostringstream stream;
|
||
|
||
// 设置流的精度为35位有效数字，并使用科学计数法
|
||
stream << std::scientific << std::setprecision(35) << value;
|
||
|
||
// 将标准字符串转换为 QString
|
||
return QString::fromStdString(stream.str());
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
void initializeMatrixWithSSE2(Eigen::MatrixXd& mat, const double* data, long rowcount, long colcount) {
|
||
__m128d zero = _mm_setzero_pd();
|
||
|
||
#pragma omp parallel for
|
||
for (long i = 0; i < rowcount; ++i) {
|
||
for (long j = 0; j < colcount; j += 2) { // 每次处理2个double
|
||
if (j + 2 <= colcount) {
|
||
__m128d src = _mm_loadu_pd(&data[i * colcount + j]);
|
||
_mm_storeu_pd(&mat(i, j), src);
|
||
}
|
||
else {
|
||
// 处理剩余部分
|
||
for (long k = j; k < colcount; ++k) {
|
||
mat(i, k) = data[i * colcount + k];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void initializeMatrixWithSSE2(Eigen::MatrixXf& mat, const float* data, long rowcount, long colcount) {
|
||
__m128 zero = _mm_setzero_ps();
|
||
|
||
#pragma omp parallel for
|
||
for (long i = 0; i < rowcount; ++i) {
|
||
for (long j = 0; j < colcount; j += 4) { // 每次处理4个float
|
||
if (j + 4 <= colcount) {
|
||
__m128 src = _mm_loadu_ps(&data[i * colcount + j]);
|
||
_mm_storeu_ps(&mat(i, j), src);
|
||
}
|
||
else {
|
||
// 处理剩余部分
|
||
for (long k = j; k < colcount; ++k) {
|
||
mat(i, k) = data[i * colcount + k];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
Eigen::MatrixXd BASECONSTVARIABLEAPI MuhlemanSigmaArray(Eigen::MatrixXd& eta_deg)
|
||
{
|
||
Eigen::MatrixXd sigma = Eigen::MatrixXd::Zero(eta_deg.rows(), eta_deg.cols());
|
||
for (long i = 0; i < sigma.rows(); i++) {
|
||
for (long j = 0; j < sigma.cols(); j++) {
|
||
sigma(i,j) = calculate_MuhlemanSigma(eta_deg(i, j));
|
||
}
|
||
}
|
||
return sigma;
|
||
}
|
||
|
||
Eigen::MatrixXd BASECONSTVARIABLEAPI dB2Amp(Eigen::MatrixXd& sigma0)
|
||
{
|
||
Eigen::MatrixXd sigma = Eigen::MatrixXd::Zero(sigma0.rows(), sigma0.cols());
|
||
for (long i = 0; i < sigma.rows(); i++) {
|
||
for (long j = 0; j < sigma.cols(); j++) {
|
||
sigma(i, j) =std::pow(10.0, sigma0(i,j)/20.0);
|
||
}
|
||
}
|
||
return sigma;
|
||
}
|
||
|
||
|
||
|
||
QDateTime parseCustomDateTime(const QString& dateTimeStr) {
|
||
// 手动解析日期时间字符串
|
||
int year = dateTimeStr.left(4).toInt();
|
||
int month = dateTimeStr.mid(5, 2).toInt();
|
||
int day = dateTimeStr.mid(8, 2).toInt();
|
||
int hour = dateTimeStr.mid(11, 2).toInt();
|
||
int minute = dateTimeStr.mid(14, 2).toInt();
|
||
int second = dateTimeStr.mid(17, 2).toInt();
|
||
int msec = dateTimeStr.mid(20, 6).toInt(); // 只取毫秒的前三位，因为QDateTime支持到毫秒
|
||
|
||
// 创建 QDate 和 QTime 对象
|
||
QDate date(year, month, day);
|
||
QTime time(hour, minute, second, msec ); // 转换为微秒，但QTime只支持毫秒精度
|
||
|
||
// 构造 QDateTime 对象
|
||
QDateTime result(date, time);
|
||
|
||
return result;
|
||
}
|
||
|
||
|
||
bool isLeapYear(int year) {
|
||
return (year % 4 == 0 && (year % 100 != 0 || year % 400 == 0));
|
||
}
|
||
|
||
int daysInMonth(int year, int month) {
|
||
if (month == 2) return isLeapYear(year) ? 29 : 28;
|
||
else if (month == 4 || month == 6 || month == 9 || month == 11) return 30;
|
||
else return 31;
|
||
}
|
||
|
||
|
||
TimestampMicroseconds parseAndConvert( std::string dateTimeStr) {
|
||
// 解析年、月、日、时、分、秒和微秒
|
||
int year = std::stoi(dateTimeStr.substr(0, 4));
|
||
int month = std::stoi(dateTimeStr.substr(5, 2));
|
||
int day = std::stoi(dateTimeStr.substr(8, 2));
|
||
int hour = std::stoi(dateTimeStr.substr(11, 2));
|
||
int minute = std::stoi(dateTimeStr.substr(14, 2));
|
||
int second = std::stoi(dateTimeStr.substr(17, 2));
|
||
int microsec = std::stoi(dateTimeStr.substr(20, 6));
|
||
|
||
// 计算从1970年至目标年份前一年的总天数
|
||
long long totalDays = 0;
|
||
for (int y = 1970; y < year; ++y) {
|
||
totalDays += isLeapYear(y) ? 366 : 365;
|
||
}
|
||
|
||
// 加上目标年份从1月到上一个月的天数
|
||
for (int m = 1; m < month; ++m) {
|
||
totalDays += daysInMonth(year, m);
|
||
}
|
||
|
||
// 加上本月的天数
|
||
totalDays += day - 1;
|
||
|
||
// 转换为总秒数，再加上小时、分钟、秒
|
||
long long totalSeconds = totalDays * 24 * 60 * 60 + hour * 60 * 60 + minute * 60 + second;
|
||
|
||
// 转换为毫秒和微秒
|
||
long long msecsSinceEpoch = totalSeconds * 1000 + microsec / 1000;
|
||
int microseconds = microsec % 1000;
|
||
|
||
return { msecsSinceEpoch, microseconds };
|
||
}
|
||
|
||
|