KJ135
/
simorthoprogram
mirror of http://172.16.0.12:5000/WBJY/simorthoprogram.git
4
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
simorthoprogram/simorthoprogram-orth_gf3-strip/baseTool.cpp

1402 lines
50 KiB
C++
Raw Blame History

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.

#pragma once
///
/// <20><><EFBFBD><EFBFBD><EFBFBD>ࡢ<EFBFBD><E0A1A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
///
//#define EIGEN_USE_MKL_ALL
//#define EIGEN_VECTORIZE_SSE4_2
//#include <mkl.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
//#include <mkl.h>
#include <string>
#include <omp.h>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#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 <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/features2d.hpp>
#include <fstream>
#include <proj.h>
#include "baseTool.h"
using namespace std;
using namespace Eigen;
using namespace cv;
/**
* @brief GetCurrentTime <20><>ȡ<EFBFBD><C8A1>ǰʱ<C7B0><CAB1>
* @return
*/
std::string 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);
std::string strTime = mbstr;
return strTime;
}
std::string 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);
std::string strTime = mbstr;
return strTime;
}
Landpoint operator +(const Landpoint& p1, const Landpoint& p2)
{
return Landpoint{ p1.lon + p2.lon,p1.lat + p2.lat,p1.ati + p2.ati };
}
Landpoint operator -(const Landpoint& p1, const Landpoint& p2)
{
return Landpoint{ p1.lon - p2.lon,p1.lat - p2.lat,p1.ati - p2.ati };
}
bool operator ==(const Landpoint& p1, const Landpoint& p2)
{
return p1.lat == p2.lat && p1.lon == p2.lon && p1.ati == p2.ati;
}
Landpoint operator *(const Landpoint& p, double scale)
{
return Landpoint{
p.lon * scale,
p.lat * scale,
p.ati * scale
};
}
double getAngle(const Landpoint& a, const Landpoint& b)
{
double c = dot(a, b) / (getlength(a) * getlength(b));
if (a.lon * b.lat - a.lat * b.lon >= 0) {
return acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
}
else {
return 360 - acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
}
}
double dot(const Landpoint& p1, const Landpoint& p2)
{
return p1.lat * p2.lat + p1.lon * p2.lon + p1.ati * p2.ati;
}
double getlength(const Landpoint& p1) {
return sqrt(dot(p1, p1));
}
Landpoint crossProduct(const Landpoint& a, const Landpoint& b) {
return Landpoint{
a.lat * b.ati - a.ati * b.lat,//x
a.ati * b.lon - a.lon * b.ati,//y
a.lon * b.lat - a.lat * b.lon//z
};
}
/// <summary>
/// <20><><EFBFBD><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><C2B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㶼<EFBFBD><E3B6BC>WGS84<38><34><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>p1Ϊ<31><CEAA><EFBFBD>㣬
/// <20><> N
/// W p1
/// p4 p0 p2 E
/// p3 S
/// n21=n2xn1
/// n=(n21+n32+n43+n41)*0.25;
/// </summary>
/// <param name="p0">Ŀ<><C4BF><EFBFBD><EFBFBD>?1<><31>?7</param>
/// <param name="p1"><3E><>Χ<EFBFBD><CEA7>1</param>
/// <param name="p2"><3E><>Χ<EFBFBD><CEA7>2</param>
/// <param name="p3"><3E><>Χ<EFBFBD><CEA7>3</param>
/// <param name="p4"><3E><>Χ<EFBFBD><CEA7>4</param>
/// <returns><3E><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD></returns>
Landpoint getSlopeVector(const Landpoint& p0, const Landpoint& p1, const Landpoint& p2, const Landpoint& p3, const Landpoint& p4) {
Landpoint n0 = LLA2XYZ(p0),
n1 = LLA2XYZ(p1),
n2 = LLA2XYZ(p2),
n3 = LLA2XYZ(p3),
n4 = LLA2XYZ(p4);
Landpoint n01 = n1 - n0, n02 = n2 - n0, n03 = n3 - n0, n04 = n4 - n0;
// <20><>n01Ϊ<31><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Landpoint np01 = p1-p0, np02 = p2-p0, np03 = p3-p0, np04 = p4-p0;
double a2 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np02.lon,np02.lat,0 });// 01->02 <20><>ʱ<EFBFBD><CAB1>
double a3 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np03.lon,np03.lat,0 });// 01->03 <20><>ʱ<EFBFBD><CAB1>
double a4 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np04.lon,np04.lat,0 });// 01->04 <20><>ʱ<EFBFBD><CAB1>
//std::cout << a2 << "\t" << a3 << "\t" << a4 << endl;
a2 = 360 - a2;
a3 = 360 - a3;
a4 = 360 - a4;
Landpoint N, W, S, E;
N = n01;
if (a2 >= a3 && a2 >= a4) {
W = n02;
if (a3 >= a4) {
S = n03;
E = n04;
}
else {
S = n04;
E = n03;
}
}
else if (a3 >= a2 && a3 >= a4) {
W = n03;
if (a2 >= a4) {
S = n02;
E = n04;
}
else {
S = n04;
E = n02;
}
}
else if (a4 >= a2 && a4 >= a3)
{
W = n04;
if (a2 >= a3) {
S = n02;
E = n03;
}
else {
S = n03;
E = n02;
}
}
return (crossProduct(N, W) + crossProduct(W, S) + crossProduct(S, E) + crossProduct(E, N)) * 0.25;
}
/// <summary>
/// <20><><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С
/// p11 p12 p13 p14
/// p21 p22 p23 p24
/// p(2+u,2+v)
/// p31 p32 p33 p34
/// p41 p42 p43 p44
/// </summary>
/// <param name="u"><3E><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7</param>
/// <param name="v"><3E><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7</param>
/// <param name="img">4x4 <20><>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD></param>
/// <returns></returns>
complex<double> Cubic_Convolution_interpolation(double u, double v, Eigen::MatrixX<complex<double>> img)
{
if (img.rows() != 4 || img.cols() != 4) {
throw exception("the size of img's block is not right");
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>
Eigen::MatrixX<complex<double>> wrc(1, 4);// ʹ<><CAB9> complex<double> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫԭ<D2AA><D4AD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˻<EFBFBD>ȡֵ
Eigen::MatrixX<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<complex<double>> interValue = wrc * img * wcr;
return interValue(0, 0);
}
complex<double> Cubic_kernel_weight(double s)
{
s = abs(s);
if (s <= 1) {
return complex<double>(1.5 * s * s * s - 2.5 * s * s + 1,0);
}
else if (s <= 2) {
return complex<double>(-0.5 * s * s * s + 2.5 * s * s - 4 * s + 2,0);
}
else {
return 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;
}
/// <summary>
/// <20><><EFBFBD><EFBFBD>γ<EFBFBD><CEB3>ת<EFBFBD><D7AA>Ϊ<EFBFBD>ع̲<D8B9><CCB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
/// </summary>
/// <param name="XYZP"><3E><>γ<EFBFBD>ȵ<EFBFBD>--degree</param>
/// <returns>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5></returns>
Landpoint LLA2XYZ(const Landpoint& LLA) {
double L = LLA.lon * d2r;
double B = LLA.lat * d2r;
double H = LLA.ati;
double sinB = sin(B);
double cosB = cos(B);
//double N = a / sqrt(1 - e * e * sin(B) * sin(B));
double N = a / sqrt(1 - eSquare * sinB * sinB);
Landpoint result = { 0,0,0 };
result.lon = (N + H) * cosB * cos(L);
result.lat = (N + H) * cosB * sin(L);
//result.z = (N * (1 - e * e) + H) * sin(B);
result.ati = (N * (1 - 1/f_inverse)* (1 - 1 / f_inverse) + H) * sinB;
return result;
}
/// <summary>
/// <20><><EFBFBD><EFBFBD>n,3) lon,lat,ati
/// </summary>
/// <param name="landpoint"></param>
/// <returns></returns>
Eigen::MatrixXd LLA2XYZ(Eigen::MatrixXd landpoint)
{
landpoint.col(0) = landpoint.col(0).array() * d2r; // lon
landpoint.col(1) = landpoint.col(1).array() * d2r; // lat
Eigen::MatrixXd sinB = (landpoint.col(1).array().sin());//lat
Eigen::MatrixXd cosB = (landpoint.col(1).array().cos());//lat
Eigen::MatrixXd N = a / ((1 - sinB.array().pow(2) * eSquare).array().sqrt());
Eigen::MatrixXd result(landpoint.rows(), 3);
result.col(0) = (N.array() + landpoint.col(2).array()) * cosB.array() * Eigen::cos(landpoint.col(0).array()).array(); //x
result.col(1) = (N.array() + landpoint.col(2).array()) * cosB.array() * Eigen::sin(landpoint.col(0).array()).array(); //y
result.col(2) = (N.array() * (1 - 1/f_inverse)* (1 - 1 / f_inverse) + landpoint.col(2).array()) * sinB.array(); //z
return result;
}
/// <summary>
/// <20><><EFBFBD>ع̲<D8B9><CCB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA>Ϊ<EFBFBD><CEAA>γ<EFBFBD><CEB3>
/// </summary>
/// <param name="XYZ"><3E>̲<EFBFBD><CCB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ</param>
/// <returns><3E><>γ<EFBFBD><CEB3>--degree</returns>
Landpoint XYZ2LLA(const Landpoint& XYZ) {
double tmpX = XYZ.lon;// <20><><EFBFBD><EFBFBD> x
double temY = XYZ.lat;// γ<><CEB3> y
double temZ = XYZ.ati;
double curB = 0;
double N = 0;
double sqrtTempXY = sqrt(tmpX * tmpX + temY * temY);
double calB = atan2(temZ, sqrtTempXY);
int counter = 0;
double sinCurB = 0;
while (abs(curB - calB) * r2d > epsilon && counter < 25)
{
curB = calB;
sinCurB = sin(curB);
N = a / sqrt(1 - eSquare * sinCurB * sinCurB);
calB = atan2(temZ + N * eSquare * sinCurB, sqrtTempXY);
counter++;
}
Landpoint result = { 0,0,0 };
result.lon = atan2(temY, tmpX) * r2d;
result.lat = curB * r2d;
result.ati = temZ / sinCurB - N * (1 - eSquare);
return result;
}
/// <summary>
/// <20><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>ȡӰ<C8A1><D3B0><EFBFBD>?1<><31>?7
/// </summary>
/// <param name="dem_path"></param>
gdalImage::gdalImage(string raster_path)
{
omp_lock_t lock;
omp_init_lock(&lock); // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
omp_set_lock(&lock); //<2F><>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7
this->img_path = raster_path;
GDALAllRegister();// ע<><D7A2><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7
// <20><>DEMӰ<4D><D3B0>
GDALDataset* rasterDataset = (GDALDataset*)(GDALOpen(raster_path.c_str(), GA_ReadOnly));//<2F><>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>
this->width = rasterDataset->GetRasterXSize();
this->height = rasterDataset->GetRasterYSize();
this->band_num = rasterDataset->GetRasterCount();
double* gt = new double[6];
// <20><>÷<EFBFBD><C3B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rasterDataset->GetGeoTransform(gt);
this->gt = Eigen::MatrixXd(2, 3);
this->gt << gt[0], gt[1], gt[2], gt[3], gt[4], gt[5];
this->projection = rasterDataset->GetProjectionRef();
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//double* inv_gt = new double[6];;
//GDALInvGeoTransform(gt, inv_gt); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD>ͶӰ
GDALFlushCache((GDALDatasetH)rasterDataset);
GDALClose((GDALDatasetH)rasterDataset);
rasterDataset = NULL;// ָ<><D6B8><EFBFBD>ÿ<EFBFBD>
this->InitInv_gt();
delete[] gt;
////GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
omp_unset_lock(&lock); //<2F>ͷŻ<CDB7><C5BB><EFBFBD><EFBFBD><EFBFBD>
omp_destroy_lock(&lock); //<2F><><EFBFBD>ٻ<EFBFBD><D9BB><EFBFBD><EFBFBD><EFBFBD>
}
gdalImage::~gdalImage()
{
}
void gdalImage::setHeight(int height)
{
this->height = height;
}
void gdalImage::setWidth(int width)
{
this->width = width;
}
void gdalImage::setTranslationMatrix(Eigen::MatrixXd gt)
{
this->gt = gt;
}
void gdalImage::setData(Eigen::MatrixXd data)
{
this->data = data;
}
Eigen::MatrixXd gdalImage::getData(int start_row, int start_col, int rows_count, int cols_count, int band_ids = 1)
{
omp_lock_t lock;
omp_init_lock(&lock);
omp_set_lock(&lock);
GDALAllRegister();
GDALDataset* rasterDataset = (GDALDataset*)(GDALOpen(this->img_path.c_str(), GA_ReadOnly));//<2F><>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>
GDALDataType gdal_datatype = rasterDataset->GetRasterBand(1)->GetRasterDataType();
GDALRasterBand* demBand = rasterDataset->GetRasterBand(band_ids);
rows_count = start_row + rows_count <= this->height ? rows_count : this->height - start_row;
cols_count = start_col + cols_count <= this->width ? cols_count : this->width - start_col;
MatrixXd datamatrix(rows_count, cols_count);
if (gdal_datatype == GDALDataType::GDT_UInt16) {
unsigned short* temp = new unsigned short[rows_count * cols_count];
demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
for (int i = 0; i < rows_count; i++) {
for (int j = 0; j < cols_count; j++) {
datamatrix(i, j) = temp[i * cols_count + j];
}
}
delete[] temp;
}
else if (gdal_datatype == GDALDataType::GDT_Int16) {
short* temp = new short[rows_count * cols_count];
demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
for (int i = 0; i < rows_count; i++) {
for (int j = 0; j < cols_count; j++) {
datamatrix(i, j) = temp[i * cols_count + j];
}
}
delete[] temp;
}
else if (gdal_datatype == GDALDataType::GDT_Float32) {
float* temp = new float[rows_count * cols_count];
demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
for (int i = 0; i < rows_count; i++) {
for (int j = 0; j < cols_count; j++) {
datamatrix(i, j) = temp[i * cols_count + j];
}
}
delete[] temp;
}
GDALClose((GDALDatasetH)rasterDataset);
omp_unset_lock(&lock); //<2F>ͷŻ<CDB7><C5BB><EFBFBD><EFBFBD><EFBFBD>
omp_destroy_lock(&lock); //<2F><><EFBFBD>ٻ<EFBFBD><D9BB><EFBFBD><EFBFBD><EFBFBD>
//GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
return datamatrix;
}
/// <summary>
/// д<><D0B4>ң<EFBFBD><D2A3>Ӱ<EFBFBD><D3B0>
/// </summary>
/// <param name="data"></param>
/// <param name="start_row"></param>
/// <param name="start_col"></param>
/// <param name="band_ids"></param>
void gdalImage::saveImage(Eigen::MatrixXd data, int start_row = 0, int start_col = 0, int band_ids = 1)
{
omp_lock_t lock;
omp_init_lock(&lock);
omp_set_lock(&lock);
if (start_row + data.rows() > this->height || start_col + data.cols() > this->width) {
string tip = "file path: " + this->img_path;
throw exception(tip.c_str());
}
GDALAllRegister();
GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
GDALDataset* poDstDS = nullptr;
if (boost::filesystem::exists(this->img_path)) {
poDstDS = (GDALDataset*)(GDALOpen(this->img_path.c_str(), GA_Update));
}
else {
poDstDS = poDriver->Create(this->img_path.c_str(), this->width, this->height, this->band_num, GDT_Float32, NULL); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
poDstDS->SetProjection(this->projection.c_str());
double gt_ptr[6];
for (int i = 0; i < this->gt.rows(); i++) {
for (int j = 0; j < this->gt.cols(); j++) {
gt_ptr[i * 3 + j] = this->gt(i, j);
}
}
poDstDS->SetGeoTransform(gt_ptr);
delete[] gt_ptr;
}
int datarows = data.rows();
int datacols = data.cols();
float* databuffer = new float[datarows * datacols];// (float*)malloc(datarows * datacols * sizeof(float));
for (int i = 0; i < datarows; i++) {
for (int j = 0; j < datacols; j++) {
float temp = float(data(i, j));
databuffer[i * datacols + j] = temp;
}
}
//poDstDS->RasterIO(GF_Write,start_col, start_row, datacols, datarows, databuffer, datacols, datarows, GDT_Float32,band_ids, num,0,0,0);
poDstDS->GetRasterBand(band_ids)->RasterIO(GF_Write, start_col, start_row, datacols, datarows, databuffer, datacols, datarows, GDT_Float32, 0, 0);
poDstDS->GetRasterBand(band_ids)->SetNoDataValue(-9999);
GDALFlushCache(poDstDS);
GDALClose((GDALDatasetH)poDstDS);
//GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
delete[] databuffer;
omp_unset_lock(&lock); //<2F>ͷŻ<CDB7><C5BB><EFBFBD><EFBFBD><EFBFBD>
omp_destroy_lock(&lock); //<2F><><EFBFBD>ٻ<EFBFBD><D9BB><EFBFBD><EFBFBD><EFBFBD>
}
void gdalImage::saveImage()
{
this->saveImage(this->data, this->start_row, this->start_col, this->data_band_ids);
}
void gdalImage::setNoDataValue(double nodatavalue = -9999, int band_ids = 1)
{
GDALAllRegister();// ע<><D7A2><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7
//GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
GDALDataset* poDstDS = (GDALDataset*)(GDALOpen(img_path.c_str(), GA_Update));
poDstDS->GetRasterBand(band_ids)->SetNoDataValue(nodatavalue);
GDALFlushCache((GDALDatasetH)poDstDS);
GDALClose((GDALDatasetH)poDstDS);
}
int gdalImage::InitInv_gt()
{
//1 lon lat = x
//1 lon lat = y
Eigen::MatrixXd temp(2, 3);
this->inv_gt = temp;
double a = this->gt(0, 0);
double b = this->gt(0, 1);
double c = this->gt(0, 2);
double d = this->gt(1, 0);
double e = this->gt(1, 1);
double f = this->gt(1, 2);
double g = 1;
double det_gt = b * f - c * e;
if (det_gt == 0) {
return 0;
}
this->inv_gt(0, 0) = (c * d - a * f) / det_gt; //2
this->inv_gt(0, 1) = f / det_gt; // lon
this->inv_gt(0, 2) = -c / det_gt; // lat
this->inv_gt(1, 0) = (a*e-b*d) / det_gt; //1
this->inv_gt(1, 1) = -e / det_gt; // lon
this->inv_gt(1, 2) = b / det_gt; // lat
return 1;
}
Landpoint gdalImage::getRow_Col(double lon, double lat)
{
Landpoint p{ 0,0,0 };
p.lon = this->inv_gt(0, 0) + lon * this->inv_gt(0, 1) + lat * this->inv_gt(0, 2); //x
p.lat = this->inv_gt(1, 0) + lon * this->inv_gt(1, 1) + lat * this->inv_gt(1, 2); //y
return p;
}
Landpoint gdalImage::getLandPoint(double row, double col, double ati = 0)
{
Landpoint p{ 0,0,0 };
p.lon = this->gt(0, 0) + col * this->gt(0, 1) + row * this->gt(0, 2); //x
p.lat = this->gt(1, 0) + col * this->gt(1, 1) + row * this->gt(1, 2); //y
p.ati = ati;
return p;
}
double gdalImage::mean(int bandids )
{
double mean_value = 0;
double count = this->height* this->width;
int line_invert = 100;
int start_ids = 0;
do {
Eigen::MatrixXd sar_a = this->getData(start_ids, 0, line_invert, this->width, bandids);
mean_value = mean_value+ sar_a.sum() / count;
start_ids = start_ids + line_invert;
} while (start_ids < this->height);
return mean_value;
}
double gdalImage::max(int bandids)
{
double max_value = 0;
bool state_max = true;
int line_invert = 100;
int start_ids = 0;
double temp_max = 0;
do {
Eigen::MatrixXd sar_a = this->getData(start_ids, 0, line_invert, this->width, bandids);
if (state_max) {
state_max = false;
max_value = sar_a.maxCoeff();
}
else {
temp_max= sar_a.maxCoeff();
if (max_value < temp_max) {
max_value = temp_max;
}
}
start_ids = start_ids + line_invert;
} while (start_ids < this->height);
return max_value;
}
double gdalImage::min(int bandids)
{
double min_value = 0;
bool state_min = true;
int line_invert = 100;
int start_ids = 0;
double temp_min = 0;
do {
Eigen::MatrixXd sar_a = this->getData(start_ids, 0, line_invert, this->width, bandids);
if (state_min) {
state_min = false;
min_value = sar_a.minCoeff();
}
else {
temp_min = sar_a.minCoeff();
if (min_value < temp_min) {
min_value = temp_min;
}
}
start_ids = start_ids + line_invert;
} while (start_ids < this->height);
return min_value;
}
GDALRPCInfo gdalImage::getRPC()
{
CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
CPLSetConfigOption("GDAL_DATA", "./data");
GDALAllRegister();//ע<><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GDALDataset* pDS = (GDALDataset*)GDALOpen(this->img_path.c_str(), GA_ReadOnly);
//<2F><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD>ȡRPC<50><43>Ϣ
char** papszRPC = pDS->GetMetadata("RPC");
//<2F><><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1>RPC<50><43>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>ɽṹ<C9BD><E1B9B9><EFBFBD>?1<><31>?7
GDALRPCInfo oInfo;
GDALExtractRPCInfo(papszRPC, &oInfo);
GDALClose((GDALDatasetH)pDS);
return oInfo;
}
Eigen::MatrixXd gdalImage::getLandPoint(Eigen::MatrixXd points)
{
if (points.cols() != 3) {
throw new exception("the size of points is equit 3!!!");
}
Eigen::MatrixXd result(points.rows(), 3);
result.col(2) = points.col(2);// <20>߳<EFBFBD>
points.col(2) = points.col(2).array() * 0 + 1;
points.col(0).swap(points.col(2));// <20><><EFBFBD><EFBFBD>
Eigen::MatrixXd gts(3, 2);
gts.col(0) = this->gt.row(0);
gts.col(1) = this->gt.row(1);
//cout << this->gt(0, 0) << "\t" << this->gt(0, 1) << "\t" << this->gt(0, 2) << endl;;
//cout << gts(0, 0) << "\t" << gts(1,0) << "\t" << gts(2, 0) << endl;;
//cout << this->gt(1, 0) << "\t" << this->gt(1, 1) << "\t" << this->gt(1, 2) << endl;;
//cout<< gts(0, 1) << "\t" << gts(1, 1) << "\t" << gts(2,1) << endl;;
//cout << points(3, 0) << "\t" << points(3, 1) << "\t" << points(3, 2) << endl;;
result.block(0, 0, points.rows(), 2) = points * gts;
return result;
}
Eigen::MatrixXd gdalImage::getHist(int bandids)
{
GDALAllRegister();// ע<><D7A2><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7
// <20><>DEMӰ<4D><D3B0>
GDALDataset* rasterDataset = (GDALDataset*)(GDALOpen(this->img_path.c_str(), GA_ReadOnly));//<2F><>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>
GDALDataType gdal_datatype = rasterDataset->GetRasterBand(1)->GetRasterDataType();
GDALRasterBand* xBand = rasterDataset->GetRasterBand(bandids);
//<2F><>ȡͼ<C8A1><CDBC>ֱ<EFBFBD><D6B1>ͼ
double dfMin = this->min(bandids);
double dfMax = this->max(bandids);
int count = int((dfMax - dfMin) / 0.01);
count = count > 255 ? count : 255;
GUIntBig* panHistogram =new GUIntBig[count];
xBand->GetHistogram(dfMin, dfMax, count, panHistogram, TRUE, FALSE, NULL, NULL);
Eigen::MatrixXd result(count, 2);
double delta = (dfMax - dfMin) / count;
for (int i = 0; i < count; i++) {
result(i, 0) = dfMin + i * delta;
result(i, 1) = double(panHistogram[i]);
}
delete[] panHistogram;
GDALClose((GDALDatasetH)rasterDataset);
return result;
}
gdalImage CreategdalImage(string img_path, int height, int width, int band_num, Eigen::MatrixXd gt, std::string projection, bool need_gt) {
if (boost::filesystem::exists(img_path.c_str())) {
boost::filesystem::remove(img_path.c_str());
}
GDALAllRegister();// ע<><D7A2><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7
GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
GDALDataset* poDstDS = poDriver->Create(img_path.c_str(), width, height, band_num, GDT_Float32, NULL); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (need_gt) {
poDstDS->SetProjection(projection.c_str());
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double gt_ptr[6] = { 0 };
for (int i = 0; i < gt.rows(); i++) {
for (int j = 0; j < gt.cols(); j++) {
gt_ptr[i * 3 + j] = gt(i, j);
}
}
poDstDS->SetGeoTransform(gt_ptr);
}
for (int i = 1; i <= band_num; i++) {
poDstDS->GetRasterBand(i)->SetNoDataValue(-9999);
}
GDALFlushCache((GDALDatasetH)poDstDS);
GDALClose((GDALDatasetH)poDstDS);
////GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
gdalImage result_img(img_path);
return result_img;
}
/// <summary>
/// <20>ü<EFBFBD>DEM
/// </summary>
/// <param name="in_path"></param>
/// <param name="out_path"></param>
/// <param name="box"></param>
/// <param name="pixelinterval"></param>
void clipGdalImage(string in_path, string out_path, DemBox box, double pixelinterval) {
}
int ResampleGDAL(const char* pszSrcFile, const char* pszOutFile, double* gt, int new_width, int new_height, GDALResampleAlg eResample)
{
GDALAllRegister();
CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
GDALDataset* pDSrc = (GDALDataset*)GDALOpen(pszSrcFile, GA_ReadOnly);
if (pDSrc == NULL)
{
return -1;
}
GDALDriver* pDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
if (pDriver == NULL)
{
GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
return -2;
}
int width = pDSrc->GetRasterXSize();
int height = pDSrc->GetRasterYSize();
int nBandCount = pDSrc->GetRasterCount();
GDALDataType dataType = pDSrc->GetRasterBand(1)->GetRasterDataType();
char* pszSrcWKT = NULL;
pszSrcWKT = const_cast<char*>(pDSrc->GetProjectionRef());
//<2F><><EFBFBD>û<EFBFBD><C3BB>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD><EFBFBD>?<3F><><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>?1<><31>?7
if (strlen(pszSrcWKT) <= 0)
{
OGRSpatialReference oSRS;
oSRS.importFromEPSG(4326);
//oSRS.SetUTM(50, true); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>120<32><30>
//oSRS.SetWellKnownGeogCS("WGS84");
oSRS.exportToWkt(&pszSrcWKT);
}
std::cout << "GDALCreateGenImgProjTransformer " << endl;
void* hTransformArg;
hTransformArg = GDALCreateGenImgProjTransformer((GDALDatasetH)pDSrc, pszSrcWKT, NULL, pszSrcWKT, FALSE, 0.0, 1);
std::cout << "no proj " << endl;
//(û<><C3BB>ͶӰ<CDB6><D3B0>Ӱ<EFBFBD><D3B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><DFB2>?<3F><><EFBFBD><EFBFBD>?1<><31>?7)
if (hTransformArg == NULL)
{
GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
return -3;
}
std::cout << "has proj " << endl;
double dGeoTrans[6] = { 0 };
int nNewWidth = 0, nNewHeight = 0;
if (GDALSuggestedWarpOutput((GDALDatasetH)pDSrc, GDALGenImgProjTransform, hTransformArg, dGeoTrans, &nNewWidth, &nNewHeight) != CE_None)
{
GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
return -3;
}
//GDALDestroyGenImgProjTransformer(hTransformArg);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC>?1<><31>?7
GDALDataset* pDDst = pDriver->Create(pszOutFile, new_width, new_height, nBandCount, dataType, NULL);
if (pDDst == NULL)
{
GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
return -2;
}
pDDst->SetProjection(pszSrcWKT);
pDDst->SetGeoTransform(gt);
GDALWarpOptions* psWo = GDALCreateWarpOptions();
//psWo->papszWarpOptions = CSLDuplicate(NULL);
psWo->eWorkingDataType = dataType;
psWo->eResampleAlg = eResample;
psWo->hSrcDS = (GDALDatasetH)pDSrc;
psWo->hDstDS = (GDALDatasetH)pDDst;
std::cout << "GDALCreateGenImgProjTransformer" << endl;
psWo->pfnTransformer = GDALGenImgProjTransform;
psWo->pTransformerArg = GDALCreateGenImgProjTransformer((GDALDatasetH)pDSrc, pszSrcWKT, (GDALDatasetH)pDDst, pszSrcWKT, FALSE, 0.0, 1);;
std::cout << "GDALCreateGenImgProjTransformer has created" << endl;
psWo->nBandCount = nBandCount;
psWo->panSrcBands = (int*)CPLMalloc(nBandCount * sizeof(int));
psWo->panDstBands = (int*)CPLMalloc(nBandCount * sizeof(int));
for (int i = 0; i < nBandCount; i++)
{
psWo->panSrcBands[i] = i + 1;
psWo->panDstBands[i] = i + 1;
}
GDALWarpOperation oWo;
if (oWo.Initialize(psWo) != CE_None)
{
GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
GDALClose((GDALDatasetH)(GDALDatasetH)pDDst);
return -3;
}
std::cout << "ChunkAndWarpImage:"<< new_width<<","<< new_height << endl;
oWo.ChunkAndWarpMulti(0, 0, new_width, new_height);
GDALFlushCache(pDDst);
std::cout << "ChunkAndWarpImage over" << endl;
//GDALDestroyGenImgProjTransformer(psWo->pTransformerArg);
//GDALDestroyWarpOptions(psWo);
GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
GDALClose((GDALDatasetH)(GDALDatasetH)pDDst);
////GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
return 0;
}
int ResampleGDALs(const char* pszSrcFile, int band_ids, GDALRIOResampleAlg eResample)
{
GDALAllRegister();
CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
GDALDataset* pDSrc = (GDALDataset*)GDALOpen(pszSrcFile, GA_Update);
if (pDSrc == NULL)
{
return -1;
}
GDALDataType gdal_datatype = pDSrc->GetRasterBand(1)->GetRasterDataType();
GDALRasterBand* demBand = pDSrc->GetRasterBand(band_ids);
int width = pDSrc->GetRasterXSize();
int height = pDSrc->GetRasterYSize();
int start_col = 0, start_row = 0, rows_count = 0, cols_count;
int row_delta = int(120000000 / width);
GDALRasterIOExtraArg psExtraArg;
INIT_RASTERIO_EXTRA_ARG(psExtraArg);
psExtraArg.eResampleAlg = eResample;
do {
rows_count = start_row + row_delta < height ? row_delta : height - start_row;
cols_count = width;
if (gdal_datatype == GDALDataType::GDT_UInt16) {
unsigned short* temp = new unsigned short[rows_count * cols_count];
demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
demBand->RasterIO(GF_Write, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0, &psExtraArg);
delete[] temp;
}
else if (gdal_datatype == GDALDataType::GDT_Int16) {
short* temp = new short[rows_count * cols_count];
demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
demBand->RasterIO(GF_Write, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0, &psExtraArg);
delete[] temp;
}
else if (gdal_datatype == GDALDataType::GDT_Float32) {
float* temp = new float[rows_count * cols_count];
demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
demBand->RasterIO(GF_Write, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0, &psExtraArg);
delete[] temp;
}
start_row = start_row + rows_count;
} while (start_row < height);
GDALClose((GDALDatasetH)pDSrc);
return 0;
}
string Convert(float Num)
{
ostringstream oss;
oss << Num;
string str(oss.str());
return str;
}
std::string JoinPath(const std::string& path, const std::string& filename)
{
int dir_size = path.size();
int last_sprt_index = path.find_last_of('\\');
int last_is_sprt = (last_sprt_index == dir_size - 1);
if (last_is_sprt)
{
return path + filename;
}
else
{
return path + "\\" + filename;
}
}
/// <summary>
////////////////////////////////////////////////////////////////////// WGS84 to J2000 /////////////////////////////////////////////////////////////////////////////////////////
/// </summary>
/*
Eigen::Matrix<double, 77, 11> WGS84_J2000::IAU2000ModelParams = Eigen::Matrix<double, 77, 11>();
WGS84_J2000::WGS84_J2000()
{
WGS84_J2000::IAU2000ModelParams << 0.0, 0.0, 0.0, 0.0, 1.0, -172064161.0, -174666.0, 33386.0, 92052331.0, 9086.0, 15377.0,
0.0, 0.0, 2.0, -2.0, 2.0, -13170906.0, -1675.0, -13696.0, 5730336.0, -3015.0, -4587.0,
0.0, 0.0, 2.0, 0.0, 2.0, -2276413.0, -234.0, 2796.0, 978459.0, -485.0, 1374.0,
0.0, 0.0, 0.0, 0.0, 2.0, 2074554.0, 207.0, -698.0, -897492.0, 470.0, -291.0,
0.0, 1.0, 0.0, 0.0, 0.0, 1475877.0, -3633.0, 11817.0, 73871.0, -184.0, -1924.0,
0.0, 1.0, 2.0, -2.0, 2.0, -516821.0, 1226.0, -524.0, 224386.0, -677.0, -174.0,
1.0, 0.0, 0.0, 0.0, 0.0, 711159.0, 73.0, -872.0, -6750.0, 0.0, 358.0,
0.0, 0.0, 2.0, 0.0, 1.0, -387298.0, -367.0, 380.0, 200728.0, 18.0, 318.0,
1.0, 0.0, 2.0, 0.0, 2.0, -301461.0, -36.0, 816.0, 129025.0, -63.0, 367.0,
0.0, -1.0, 2.0, -2.0, 2.0, 215829.0, -494.0, 111.0, -95929.0, 299.0, 132.0,
0.0, 0.0, 2.0, -2.0, 1.0, 128227.0, 137.0, 181.0, -68982.0, -9.0, 39.0,
-1.0, 0.0, 2.0, 0.0, 2.0, 123457.0, 11.0, 19.0, -53311.0, 32.0, -4.0,
-1.0, 0.0, 0.0, 2.0, 0.0, 156994.0, 10.0, -168.0, -1235.0, 0.0, 82.0,
1.0, 0.0, 0.0, 0.0, 1.0, 63110.0, 63.0, 27.0, -33228.0, 0.0, -9.0,
-1.0, 0.0, 0.0, 0.0, 1.0, -57976.0, -63.0, -189.0, 31429.0, 0.0, -75.0,
-1.0, 0.0, 2.0, 2.0, 2.0, -59641.0, -11.0, 149.0, 25543.0, -11.0, 66.0,
1.0, 0.0, 2.0, 0.0, 1.0, -51613.0, -42.0, 129.0, 26366.0, 0.0, 78.0,
-2.0, 0.0, 2.0, 0.0, 1.0, 45893.0, 50.0, 31.0, -24236.0, -10.0, 20.0,
0.0, 0.0, 0.0, 2.0, 0.0, 63384.0, 11.0, -150.0, -1220.0, 0.0, 29.0,
0.0, 0.0, 2.0, 2.0, 2.0, -38571.0, -1.0, 158.0, 16452.0, -11.0, 68.0,
0.0, -2.0, 2.0, -2.0, 2.0, 32481.0, 0.0, 0.0, -13870.0, 0.0, 0.0,
-2.0, 0.0, 0.0, 2.0, 0.0, -47722.0, 0.0, -18.0, 477.0, 0.0, -25.0,
2.0, 0.0, 2.0, 0.0, 2.0, -31046.0, -1.0, 131.0, 13238.0, -11.0, 59.0,
1.0, 0.0, 2.0, -2.0, 2.0, 28593.0, 0.0, -1.0, -12338.0, 10.0, -3.0,
-1.0, 0.0, 2.0, 0.0, 1.0, 20441.0, 21.0, 10.0, -10758.0, 0.0, -3.0,
2.0, 0.0, 0.0, 0.0, 0.0, 29243.0, 0.0, -74.0, -609.0, 0.0, 13.0,
0.0, 0.0, 2.0, 0.0, 0.0, 25887.0, 0.0, -66.0, -550.0, 0.0, 11.0,
0.0, 1.0, 0.0, 0.0, 1.0, -14053.0, -25.0, 79.0, 8551.0, -2.0, -45.0,
-1.0, 0.0, 0.0, 2.0, 1.0, 15164.0, 10.0, 11.0, -8001.0, 0.0, -1.0,
0.0, 2.0, 2.0, -2.0, 2.0, -15794.0, 72.0, -16.0, 6850.0, -42.0, -5.0,
0.0, 0.0, -2.0, 2.0, 0.0, 21783.0, 0.0, 13.0, -167.0, 0.0, 13.0,
1.0, 0.0, 0.0, -2.0, 1.0, -12873.0, -10.0, -37.0, 6953.0, 0.0, -14.0,
0.0, -1.0, 0.0, 0.0, 1.0, -12654.0, 11.0, 63.0, 6415.0, 0.0, 26.0,
-1.0, 0.0, 2.0, 2.0, 1.0, -10204.0, 0.0, 25.0, 5222.0, 0.0, 15.0,
0.0, 2.0, 0.0, 0.0, 0.0, 16707.0, -85.0, -10.0, 168.0, -1.0, 10.0,
1.0, 0.0, 2.0, 2.0, 2.0, -7691.0, 0.0, 44.0, 3268.0, 0.0, 19.0,
-2.0, 0.0, 2.0, 0.0, 0.0, -11024.0, 0.0, -14.0, 104.0, 0.0, 2.0,
0.0, 1.0, 2.0, 0.0, 2.0, 7566.0, -21.0, -11.0, -3250.0, 0.0, -5.0,
0.0, 0.0, 2.0, 2.0, 1.0, -6637.0, -11.0, 25.0, 3353.0, 0.0, 14.0,
0.0, -1.0, 2.0, 0.0, 2.0, -7141.0, 21.0, 8.0, 3070.0, 0.0, 4.0,
0.0, 0.0, 0.0, 2.0, 1.0, -6302.0, -11.0, 2.0, 3272.0, 0.0, 4.0,
1.0, 0.0, 2.0, -2.0, 1.0, 5800.0, 10.0, 2.0, -3045.0, 0.0, -1.0,
2.0, 0.0, 2.0, -2.0, 2.0, 6443.0, 0.0, -7.0, -2768.0, 0.0, -4.0,
-2.0, 0.0, 0.0, 2.0, 1.0, -5774.0, -11.0, -15.0, 3041.0, 0.0, -5.0,
2.0, 0.0, 2.0, 0.0, 1.0, -5350.0, 0.0, 21.0, 2695.0, 0.0, 12.0,
0.0, -1.0, 2.0, -2.0, 1.0, -4752.0, -11.0, -3.0, 2719.0, 0.0, -3.0,
0.0, 0.0, 0.0, -2.0, 1.0, -4940.0, -11.0, -21.0, 2720.0, 0.0, -9.0,
-1.0, -1.0, 0.0, 2.0, 0.0, 7350.0, 0.0, -8.0, -51.0, 0.0, 4.0,
2.0, 0.0, 0.0, -2.0, 1.0, 4065.0, 0.0, 6.0, -2206.0, 0.0, 1.0,
1.0, 0.0, 0.0, 2.0, 0.0, 6579.0, 0.0, -24.0, -199.0, 0.0, 2.0,
0.0, 1.0, 2.0, -2.0, 1.0, 3579.0, 0.0, 5.0, -1900.0, 0.0, 1.0,
1.0, -1.0, 0.0, 0.0, 0.0, 4725.0, 0.0, -6.0, -41.0, 0.0, 3.0,
-2.0, 0.0, 2.0, 0.0, 2.0, -3075.0, 0.0, -2.0, 1313.0, 0.0, -1.0,
3.0, 0.0, 2.0, 0.0, 2.0, -2904.0, 0.0, 15.0, 1233.0, 0.0, 7.0,
0.0, -1.0, 0.0, 2.0, 0.0, 4348.0, 0.0, -10.0, -81.0, 0.0, 2.0,
1.0, -1.0, 2.0, 0.0, 2.0, -2878.0, 0.0, 8.0, 1232.0, 0.0, 4.0,
0.0, 0.0, 0.0, 1.0, 0.0, -4230.0, 0.0, 5.0, -20.0, 0.0, -2.0,
-1.0, -1.0, 2.0, 2.0, 2.0, -2819.0, 0.0, 7.0, 1207.0, 0.0, 3.0,
-1.0, 0.0, 2.0, 0.0, 0.0, -4056.0, 0.0, 5.0, 40.0, 0.0, -2.0,
0.0, -1.0, 2.0, 2.0, 2.0, -2647.0, 0.0, 11.0, 1129.0, 0.0, 5.0,
-2.0, 0.0, 0.0, 0.0, 1.0, -2294.0, 0.0, -10.0, 1266.0, 0.0, -4.0,
1.0, 1.0, 2.0, 0.0, 2.0, 2481.0, 0.0, -7.0, -1062.0, 0.0, -3.0,
2.0, 0.0, 0.0, 0.0, 1.0, 2179.0, 0.0, -2.0, -1129.0, 0.0, -2.0,
-1.0, 1.0, 0.0, 1.0, 0.0, 3276.0, 0.0, 1.0, -9.0, 0.0, 0.0,
1.0, 1.0, 0.0, 0.0, 0.0, -3389.0, 0.0, 5.0, 35.0, 0.0, -2.0,
1.0, 0.0, 2.0, 0.0, 0.0, 3339.0, 0.0, -13.0, -107.0, 0.0, 1.0,
-1.0, 0.0, 2.0, -2.0, 1.0, -1987.0, 0.0, -6.0, 1073.0, 0.0, -2.0,
1.0, 0.0, 0.0, 0.0, 2.0, -1981.0, 0.0, 0.0, 854.0, 0.0, 0.0,
-1.0, 0.0, 0.0, 1.0, 0.0, 4026.0, 0.0, -353.0, -553.0, 0.0, -139.0,
0.0, 0.0, 2.0, 1.0, 2.0, 1660.0, 0.0, -5.0, -710.0, 0.0, -2.0,
-1.0, 0.0, 2.0, 4.0, 2.0, -1521.0, 0.0, 9.0, 647.0, 0.0, 4.0,
-1.0, 1.0, 0.0, 1.0, 1.0, 1314.0, 0.0, 0.0, -700.0, 0.0, 0.0,
0.0, -2.0, 2.0, -2.0, 1.0, -1283.0, 0.0, 0.0, 672.0, 0.0, 0.0,
1.0, 0.0, 2.0, 2.0, 1.0, -1331.0, 0.0, 8.0, 663.0, 0.0, 4.0,
-2.0, 0.0, 2.0, 2.0, 2.0, 1383.0, 0.0, -2.0, -594.0, 0.0, -2.0,
-1.0, 0.0, 0.0, 0.0, 2.0, 1405.0, 0.0, 4.0, -610.0, 0.0, 2.0,
1.0, 1.0, 2.0, -2.0, 2.0, 1290.0, 0.0, 0.0, -556.0, 0.0, 0.0;
}
WGS84_J2000::~WGS84_J2000()
{
}
/// <summary>
/// step1 WGS 84 ת<><D7AA><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>?1<><31>?7
/// <20><>ؾ<EFBFBD>γ<EFBFBD>ȸ߳<C8B8>BLH<4C><48>ع<EFBFBD><D8B9><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>?<3F><><EFBFBD><EFBFBD>?1<><31>?7 BLH-- > XG
/// ECEF, Earth Centered Earth Fixed; <20>ع<EFBFBD><D8B9><EFBFBD><EFBFBD><EFBFBD>ϵ <20>ο<EFBFBD>ƽ<EFBFBD>棺ƽ<E6A3BA><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?<3F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>CIO<49><4F><EFBFBD><EFBFBD><EFBFBD>ߴ<EFBFBD>ֱ<EFBFBD><D6B1>ƽ<EFBFBD>档
/// x<><78>Ϊ<EFBFBD>ο<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>潻<EFBFBD>ߣ<EFBFBD>z<EFBFBD><7A>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>CIO<49>㡣
/// </summary>
/// <param name="LBH_deg_m">B L H<>ֱ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>γ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ؾ<EFBFBD><D8BE>Ⱥʹ<C8BA>ظ߳<D8B8><DFB3>?1<><31>?7 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪN * 3<><33><EFBFBD><EFBFBD></param>
/// <returns>XYZ Ϊ<>ع<EFBFBD><D8B9><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5> x y z<><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪN * 3<><33><EFBFBD><EFBFBD></returns>
Eigen::MatrixXd WGS84_J2000::WGS84TECEF(Eigen::MatrixXd LBH_deg_m)
{
return LLA2XYZ(LBH_deg_m);
}
/// <summary>
/// step 2 Э<><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7 ת<><D7AA>Ϊ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
/// <20><><EFBFBD><EFBFBD>Ҫ<EFBFBD>漰<EFBFBD><E6BCB0>ѯ <20><><EFBFBD><EFBFBD>ʱ<EFBFBD>̵<EFBFBD> xp,yp , <20><><EFBFBD>Բ<EFBFBD>ѯEOP<4F>ļ<EFBFBD><C4BC><EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD>ص<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>http://celestrak.com/spacedata/
/// earthFixedXYZ=ordinateSingleRotate('x',yp)*ordinateSingleRotate('y',xp)*earthFixedXYZ;
/// </summary>
/// <param name="axis">axis <20><>ʾΧ<CABE><CEA7><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> '1' <20><>ʾΧ<CABE><CEA7> x<><78><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ת;'2' <20><>ʾΧ<CABE><CEA7> y<><79><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ת;'3'<27><>ʾΧ<CABE><CEA7> z<><7A><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ת</param>
/// <param name="angle_deg">angle_deg <20><>ʾ<EFBFBD><CABE>ת<EFBFBD>ĽǶ<C4BD> Ĭ<><C4AC> degree</param>
/// <returns></returns>
Eigen::MatrixXd WGS84_J2000::ordinateSingleRotate(int axis, double angle_deg)
{
angle_deg = angle_deg * d2r;
Eigen::MatrixXd R = Eigen::MatrixXd::Ones(3, 3);
switch (axis)
{
case 1: // x
R << 1, 0.0, 0.0,
0.0, cos(angle_deg), sin(angle_deg),
0.0, -1 * sin(angle_deg), cos(angle_deg);
break;
case 2:// y
R << cos(angle_deg), 0.0, -1 * sin(angle_deg),
0.0, 1, 0.0,
sin(angle_deg), 0.0, cos(angle_deg);
break;
case 3:// z
R << cos(angle_deg), sin(angle_deg), 0.0,
-1 * sin(angle_deg), cos(angle_deg), 0.0,
0.0, 0.0, 1;
break;
default:
throw exception("Error Axis");
exit(-1);
}
return R;
}
/// <summary>
/// step 3 ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ ת<><D7AA>Ϊ ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
/// <20><>utcʱ<63><CAB1>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD><CEBA><EFBFBD>ʱ
/// xyz= ordinateSingleRotate('z',-gst_deg)*earthFixedXYZ;
/// UNTITLED <20><><EFBFBD>㵱<EFBFBD>غ<EFBFBD><D8BA><EFBFBD>ʱ,<2C><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ʱ<EFBFBD><CAB1>Ϊ<EFBFBD><CEAA>λ
/// %dAT <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// % TAI<41><49><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>ʱ<EFBFBD><CAB1> <20><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>׼ TAI = UTC + dAT;% <20><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>ʱ<EFBFBD><CAB1>
/// % TT <20><><EFBFBD><EFBFBD>ʱ TT = TAI + 32.184 <20><>2014<31><34><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>?1<><31>?7;
/// % TDT <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѧʱ<D1A7><CAB1>
/// % ET <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
/// % <20><><EFBFBD><EFBFBD>ʱ = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѧʱ<D1A7><CAB1> = <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
/// %<25><><EFBFBD><EFBFBD>ʱ=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѧʱ<D1A7><CAB1>=<3D><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
/// </summary>
/// <param name="UTC"><3E><>ʽΪy m d <20><><EFBFBD><EFBFBD>d<EFBFBD><64><EFBFBD><EFBFBD>ֵΪС<CEAA><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>h m s <20><><EFBFBD><EFBFBD>sec/3600+min/60+h)/24ת<34><D7AA><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼӵ<DBBC>day <20><></param>
/// <param name="dUT1">dUT1 Ϊut1-utc <20>IJ<C4B2><EEA3AC>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD>룬<EFBFBD><EBA3AC>iers<72>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7 0</param>
/// <param name="dAT"><3E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 37</param>
/// <param name="gst_deg"><3E><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>ʱ<EFBFBD><CAB1> <20><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>׼ TAI=UTC+dAT; %<25><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>ʱ<EFBFBD><CAB1></param>
/// <param name="JDTDB"><3E><><EFBFBD><EFBFBD>ʱ TT = TAI+32.184 <20><>2014<31><34><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>?1<><31>?7;</param>
/// <returns></returns>
int WGS84_J2000::utc2gst(Eigen::MatrixXd UTC, double dUT1, double dAT, double& gst_deg, double& JDTDB)
{
//function[gst_deg, JDTDB] = utc2gst(UTC, dUT1, dAT)
//% TDT <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѧʱ<D1A7><CAB1>
// % ET <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
// % <20><><EFBFBD><EFBFBD>ʱ = <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѧʱ<D1A7><CAB1> = <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
double J2000 = 2451545;
double JDutc = YMD2JD(UTC(0,0), UTC(0,1), UTC(0,2));
double JDUT1 = JDutc + dUT1 / 86400;// % UT1 Ϊ<><CEAA><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>UTCʱ<43><CAB1><EFBFBD><EFBFBD>dUT1<54>IJ<EFBFBD><C4B2>dUT1<54>ڸ<EFBFBD><DAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ź<EFBFBD><C5BA>л<EFBFBD><D0BB><EFBFBD>0.1<EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD>ȸ<EFBFBD><EFBFBD><EFBFBD>IERS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>󣬻<EFBFBD><EFBFBD><EFBFBD>1e - 5<>ľ<EFBFBD><C4BE>ȸ<EFBFBD><C8B8><EFBFBD><EFBFBD><EFBFBD>
double dT = 32.184 + dAT - dUT1;
double JDTT = JDUT1 + dT / 86400;
//% JDTT = YMD2JD(UTC(1), UTC(2), UTC(3)) + dUT1 / 86400;
//% <20><><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>TDB, TDB<44><42><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD>ѧʱ<D1A7><CAB1><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ʱ<EFBFBD><CAB1>߶<EFBFBD><DFB6>?1<><31>?7
double T = (JDTT - J2000) / 36525;
double temp = 0.001657 * sin(628.3076 * T + 6.2401)
+ 0.000022 * sin(575.3385 * T + 4.2970)
+ 0.000014 * sin(1256.6152 * T + 6.1969)
+ 0.000005 * sin(606.9777 * T + 4.0212)
+ 0.000005 * sin(52.9691 * T + 0.4444)
+ 0.000002 * sin(21.3299 * T + 5.5431)
+ 0.00001 * T * sin(628.3076 * T + 4.2490);
JDTDB = JDTT + temp / 86400;
//% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>UT2, UT2<54><32><EFBFBD><EFBFBD>UT1<54>Ļ<EFBFBD><C4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC>ڱ仯<DAB1><E4BBAF>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>?1<><31>?7
// %% <20><><EFBFBD><EFBFBD>UT1<54><31><EFBFBD><EFBFBD>Tb, TbΪ<62>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԪB2000.0<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ff
// % B2000 = 2451544.033;
//% Tb = (YMD2JD(UT1(1), UT1(2), UT1(3)) - B2000) / 365.2422;
//% dTs = 0.022 * sin(2 * pi * Tb) - 0.012 * cos(2 * pi * Tb) - 0.006 * sin(4 * pi * Tb) + 0.007 * cos(4 * pi * Tb);
//% dTs = dTs / 86400;
//% UT2 = UT1 + [0 0 dTs];
//% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ʱGMST; <20><>λΪ<CEBB><CEAA>
T = (JDTDB - J2000) / 36525;
double T2 = T * T;
double T3 = T2 * T;
double T4 = T3 * T;
double T5 = T4 * T;
double Du = JDUT1 - J2000;
double thita = 0.7790572732640 + 1.00273781191135448 * Du;//% <20><>J2000<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?<3F><><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7
double GMST_deg = (-0.0000000368 * T5 - 0.000029956 * T4 - 0.00000044 * T3 + 1.3915817 * T2 + 4612.156534 * T + 0.014506) / 3600 + (thita - floor(thita)) * 360;
double epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg;
WGS84_J2000::nutationInLongitudeCaculate(JDTDB, epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg);
//[epthilongA_deg, dertaPthi_deg] = nutationInLongitudeCaculate(JDTDB);
//% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?<3F><><EFBFBD><C2B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>eclipticObliquitygama<6D><61><EFBFBD><EFBFBD>λΪ<CEBB><CEAA><EFBFBD><EFBFBD>
//% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7 ̫<><CCAB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7 <20><><EFBFBD><EFBFBD>γ<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD> <20><><EFBFBD><EFBFBD>ƽ<EFBFBD>Ǿ<EFBFBD>(<28><><EFBFBD><EFBFBD>ƽ<EFBFBD>ƾ<EFBFBD> - ̫<><CCAB>ƽ<EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>ƾ<EFBFBD> <20><>λΪ<CEBB><CEAA><EFBFBD><EFBFBD>
double F_deg = (0.00000417 * T4 - 0.001037 * T3 - 12.7512 * T2 + 1739527262.8478 * T + 335779.526232) / 3600;
F_deg = fmod(F_deg, 360);
double D_deg = (-0.00003169 * T4 + 0.006593 * T3 - 6.3706 * T2 + 1602961601.2090 * T + 1072260.70369) / 3600;
D_deg = fmod(D_deg, 360);
double Omiga_deg = (-0.00005939 * T4 + 0.007702 * T3 + 7.4722 * T2 - 6962890.5431 * T + 450160.398036) / 3600;
Omiga_deg = fmod(Omiga_deg, 360);
double epthilongGama_deg = dertaPthi_deg * cosd(epthilongA_deg)
+ (0.00264096 * sind(Omiga_deg )
+ 0.00006352 * sind(2 * Omiga_deg )
+ 0.00001175 * sind(2 * F_deg - 2 * D_deg + 3 * Omiga_deg)
+ 0.00001121 * sind(2 * F_deg - 2 * D_deg + Omiga_deg)
- 0.00000455 * sind(2 * F_deg - 2 * D_deg + 2 * Omiga_deg)
+ 0.00000202 * sind(2 * F_deg + 3 * Omiga_deg)
+ 0.00000198 * sind(2 * F_deg + Omiga_deg)
- 0.00000172 * sind(3 * Omiga_deg)
- 0.00000087 * T * sind(Omiga_deg)) / 3600;
gst_deg = epthilongGama_deg + GMST_deg;
return 0;
}
/// <summary>
/// step 4 ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ ת<><D7AA>˲ʱƽ<CAB1><C6BD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>ϵ
/// <20><><EFBFBD><EFBFBD>ƽ<EFBFBD>Ƴཻ<C6B3>ǣ<EFBFBD><C7A3>ƾ<EFBFBD><C6BE><EFBFBD><C2B6><EFBFBD><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD><EFBFBD><C2B6><EFBFBD>˲ʱ<CBB2>Ƴཻ<C6B3>ǡ<EFBFBD>
/// </summary>
/// <param name="JD"><3E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD></param>
/// <param name="accuracy"><3E><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD>?<3F><><EFBFBD><EFBFBD>?1<><31>?7 <20><>ֵΪ<D6B5><CEAA>normal<61><6C> <20>͡<EFBFBD>high<67><68> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>'N'<27>͡<EFBFBD>H'<27><>ʾһ<CABE>㾫<EFBFBD>Ⱥ͸߾<CDB8><DFBE><EFBFBD> <20><>Ĭ<EFBFBD><C4AC>Ϊ<EFBFBD>߾<EFBFBD><DFBE>ȼ<EFBFBD><C8BC><EFBFBD></param>
/// <param name="epthilongA_deg">ƽ<>Ƴཻ<C6B3><E0BDBB></param>
/// <param name="dertaPthi_deg"><3E>ƾ<EFBFBD><C6BE><EFBFBD></param>
/// <param name="dertaEpthilong_deg"><3E>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD><EFBFBD></param>
/// <param name="epthilong_deg">˲ʱ<CBB2>Ƴཻ<C6B3><E0BDBB></param>
/// <returns></returns>
int WGS84_J2000::nutationInLongitudeCaculate(double JD, double& epthilongA_deg, double& dertaPthi_deg, double& dertaEpthilong_deg, double& epthilong_deg)
{
double T = (JD - 2451545) / 36525;
double T2 = T * T;
double T3 = T2 * T;
double T4 = T3 * T;
double T5 = T4 * T;
//% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>lunarMeanAnomaly_deg(l_deg) ̫<><CCAB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>SolarMeanAnomaly_deg(solarl_deg)
// % <20><><EFBFBD><EFBFBD>γ<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>lunarLatitudeAngle_deg(F_deg) <20><><EFBFBD><EFBFBD>ƽ<EFBFBD>Ǿ<EFBFBD>diffLunarSolarElestialLongitude_deg(D_deg<65><67><EFBFBD><EFBFBD>ƽ<EFBFBD>ƾ<EFBFBD> - ̫<><CCAB>ƽ<EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>
// % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>ƾ<EFBFBD>SolarAscendingNodeElestialLongitude_deg(Omiga_deg)
double l_deg = (-0.00024470 * T4 + 0.051635 * T3 + 31.8792 * T2 + 1717915923.2178 * T + 485868.249036) / 3600;
l_deg = fmod(l_deg, 360);
double solarl_deg = (-0.00001149 * T4 + 0.000136 * T3 - 0.5532 * T2 + 129596581.0481 * T + 1287104.79305) / 3600;
solarl_deg = fmod(solarl_deg, 360);
double F_deg = (0.00000417 * T4 - 0.001037 * T3 - 12.7512 * T2 + 1739527262.8478 * T + 335779.526232) / 3600;
F_deg = fmod(F_deg, 360);
double D_deg = (-0.00003169 * T4 + 0.006593 * T3 - 6.3706 * T2 + 1602961601.2090 * T + 1072260.70369) / 3600;
D_deg = fmod(D_deg, 360);
double Omiga_deg = (-0.00005939 * T4 + 0.007702 * T3 + 7.4722 * T2 - 6962890.5431 * T + 450160.398036) / 3600;
Omiga_deg = fmod(Omiga_deg, 360);
Eigen::MatrixXd basicAngle_deg = Eigen::Matrix<double, 1, 5>{ l_deg,solarl_deg,F_deg,D_deg,Omiga_deg };
epthilongA_deg = (-0.0000000434 * T5 - 0.000000576 * T4 + 0.00200340 * T3 - 0.0001831 * T2 - 46.836769 * T + 84381.406) / 3600;
epthilongA_deg = epthilongA_deg - floor(epthilongA_deg / 360) * 360;
//% IAU2000ģ<30><C4A3><EFBFBD><EFBFBD>77<37><37>
Eigen::MatrixXd elestialLonNutation = WGS84_J2000::IAU2000ModelParams;
dertaPthi_deg = -3.75e-08;
dertaEpthilong_deg = 0.388e-3 / 3600;
for (int i = 0; i < 77; i++) {
double sumAngle_deg = 0;
for (int j = 0; j < 5; j++) {
sumAngle_deg = sumAngle_deg + elestialLonNutation(i, j) * basicAngle_deg(j);
}
sumAngle_deg = sumAngle_deg - floor(sumAngle_deg / 360) * 360;
dertaPthi_deg = dertaPthi_deg + ((elestialLonNutation(i, 5) + elestialLonNutation(i, 6) * T) * sind(sumAngle_deg ) + elestialLonNutation(i, 7) * cosd(sumAngle_deg)) * 1e-7 / 3600;
dertaEpthilong_deg = dertaEpthilong_deg + ((elestialLonNutation(i, 8) + elestialLonNutation(i, 9) * T) * cosd(sumAngle_deg) + elestialLonNutation(i, 10) * sind(sumAngle_deg)) * 1e-7 / 3600;
}
epthilong_deg = epthilongA_deg + dertaEpthilong_deg;
return 0;
}
/// <summary>
/// zA<7A><41>thitaA<61><41>zetaAΪ<41><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/// </summary>
/// <param name="JDTDB"></param>
/// <param name="zetaA"></param>
/// <param name="thitaA"></param>
/// <param name="zA"></param>
/// <returns></returns>
int WGS84_J2000::precessionAngle(double JDTDB, double& zetaA, double& thitaA, double& zA)
{
double T = (JDTDB - 2451545) / 36525;
double T2 = T * T;
double T3 = T2 * T;
//%
//% zetaA = (2306.218 * T + 0.30188 * T2 + 0.017998 * T3) / 3600;
//% zA = (2306.218 * T + 1.09468 * T2 + 0.018203) / 3600;
//% thitaA = (2004.3109 * T - 0.42665 * T2 - 0.041833 * T3) / 3600;
double T4 = T3 * T;
double T5 = T4 * T;
zetaA = (-0.0000003173 * T5 - 0.000005971 * T4 + 0.01801828 * T3 + 0.2988499 * T2 + 2306.083227 * T + 2.650545) / 3600;
thitaA = (-0.0000001274 * T5 - 0.000007089 * T4 - 0.04182264 * T3 - 0.4294934 * T2 + 2004.191903 * T) / 3600;
zA = (-0.0000002904 * T5 - 0.000028596 * T4 + 0.01826837 * T3 + 1.0927348 * T2 + 2306.077181 * T - 2.650545) / 3600;
return 0;
}
/// <summary>
/// ͬʱ YMD2JD<4A><44><EFBFBD><EFBFBD>Ϊ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>գ<EFBFBD>
/// </summary>
/// <param name="yy"></param>
/// <param name="mm"></param>
/// <param name="dd"></param>
/// <returns></returns>
double WGS84_J2000::YMD2JD(double y, double m, double d)
{
int B = 0;
if (y > 1582 || (y == 1582 && m > 10) || (y == 1582 && m == 10 && d >= 15)) {
B = 2 - floor(y / 100) + floor(y / 400);
}
return floor(365.25 * (y + 4712)) + floor(30.6 * (m + 1)) + d - 63.5 + B;
}
/// <summary>
/// WGS84 ת J2000
/// </summary>
/// <param name="BLH_deg_m">WGS84 <20><>γ<EFBFBD><CEB3></param>
/// <param name="UTC">1x3 Y,m,d</param>
/// <param name="xp">EARTH ORIENTATION PARAMETER Xp</param>
/// <param name="yp">EARTH ORIENTATION PARAMETER Yp</param>
/// <param name="dut1">Solar Weather Data dut1</param>
/// <param name="dat">Solar Weather Data dat</param>
/// <returns></returns>
Eigen::MatrixXd WGS84_J2000::WGS842J2000(Eigen::MatrixXd BLH_deg_m, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat)
{
//step 1 step1 WGS 84 ת<><D7AA><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>?1<><31>?7
Eigen::MatrixXd earthFixedXYZ = WGS84_J2000::WGS84TECEF(BLH_deg_m).transpose();
//step 2 Э<><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7 ת<><D7AA>Ϊ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD>Ҫ<EFBFBD>漰<EFBFBD><E6BCB0>ѯ <20><><EFBFBD><EFBFBD>ʱ<EFBFBD>̵<EFBFBD> xp,yp , <20><><EFBFBD>Բ<EFBFBD>ѯEOP<4F>ļ<EFBFBD><C4BC><EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD>ص<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>http://celestrak.com/spacedata/
earthFixedXYZ = ordinateSingleRotate(1, yp) * ordinateSingleRotate(2, xp) * earthFixedXYZ;
//step 3 ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ ת<><D7AA>Ϊ ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
// <20>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>漰<EFBFBD><E6BCB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD><CEBA><EFBFBD>ʱ<EFBFBD>ǵļ<C7B5><C4BC>㣬<EFBFBD><E3A3AC><EFBFBD>ڸ<EFBFBD><DAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD><CEBA><EFBFBD>ʱ<EFBFBD><CAB1> <20><><EFBFBD>㷽<EFBFBD><E3B7BD> <20>ܶ࣬<DCB6><E0A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?1<><31>?7 һ<><D2BB><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ȷ<EFBFBD>ļ<EFBFBD><C4BC>㷽<EFBFBD><E3B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dut1 <20><>dat<61><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>EOP<4F>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD>С<EFBFBD>EOP<4F>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD>ص<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD> http://celestrak.org/spacedata/
double gst_deg, JDTDB;
WGS84_J2000::utc2gst(UTC, dut1, dat, gst_deg, JDTDB);
Eigen::MatrixXd xyz = ordinateSingleRotate(3, -1 * gst_deg) * earthFixedXYZ;
//step 4 ˲ʱ<CBB2><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ ת<><D7AA>˲ʱƽ<CAB1><C6BD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>ϵ
double epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg;
WGS84_J2000::nutationInLongitudeCaculate(JDTDB, epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg);
xyz = ordinateSingleRotate(1, -epthilongA_deg) * ordinateSingleRotate(3, dertaPthi_deg) * ordinateSingleRotate(1, dertaEpthilong_deg + epthilongA_deg) * xyz;
//step5 ˲ʱƽ<CAB1><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA>ΪЭ<CEAA><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>J2000<30><30>
double zetaA, thitaA, zA;
WGS84_J2000::precessionAngle(JDTDB, zetaA, thitaA, zA);
xyz = ordinateSingleRotate(3, zetaA) * ordinateSingleRotate(2, -thitaA) * ordinateSingleRotate(3, zA) * xyz;
return xyz.transpose();
//return Eigen::MatrixXd();
}
Landpoint WGS84_J2000::WGS842J2000(Landpoint p, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat)
{
Eigen::MatrixXd blh = Eigen::MatrixXd(1, 3);
blh << p.lon, p.lat, p.ati;
blh = WGS84_J2000::WGS842J2000(blh, UTC, xp, yp, dut1, dat);
return Landpoint{ blh(0,0),blh(0,1) ,blh(0,0) };
//return Landpoint();
}
*/
//<2F><><EFBFBD>㷽λ<E3B7BD><CEBB><EFBFBD><EFBFBD><EFBFBD>ӵķ<D3B5><C4B7><EFBFBD>
BLH xyz2blh(double X, double Y, double Z)
{
double f, e2, B = 0.0, N = 0.0, H = 0.0, R0, R1, deltaH, deltaB;
f = (1.0 / 298.257223563), e2 = f * (2 - f);
BLH res = { 0 };
R0 = sqrt(pow(X, 2) + pow(Y, 2));
R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
res.L = atan2(Y, X);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD>Ⱥʹ<C8BA><CDB4>ظ<EFBFBD>
N = a;
H = R1 - a;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
do
{
deltaH = N;//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
deltaB = B;
N = a / sqrt(1 - e2 * pow(sin(B), 2));
H = R0 / cos(B) - N;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
} while (fabs(deltaH - H) > 1.0e-3 && fabs(deltaB - B) > 1.0e-9);
res.B = B;
res.H = H;
return res;
}
ENU xyz2enu(double Xr, double Yr, double Zr, double Xs, double Ys, double Zs)
{
ENU enu = { 0 };
BLH ref = xyz2blh(Xr, Yr, Zr);
double sinL = sin(ref.L);
double cosL = cos(ref.L);
double sinB = sin(ref.B);
double cosB = cos(ref.B);
double dx = Xs - Xr;
double dy = Ys - Yr;
double dz = Zs - Zr;
enu.E = -sinL * dx + cosL * dy;
enu.N = -sinB * cosL * dx - sinB * sinL * dy + cosB * dz;
enu.U = cosB * cosL * dx + cosB * sinL * dy + sinB * dz;
return enu;
}
RAH Satrah(double Xr, double Yr, double Zr, double Xs, double Ys, double Zs)
{
RAH rah = { 0 };
ENU enu = xyz2enu(Xr, Yr, Zr, Xs, Ys, Zs);
rah.H = atan2(enu.U, sqrt(enu.E * enu.E + enu.N * enu.N));
rah.A = atan2(enu.E, enu.N);
if (rah.A < 0)
rah.A += 2 * M_PI;
if (rah.A > 2 * M_PI)
rah.A -= 2 * M_PI;
rah.R = sqrt(enu.E * enu.E + enu.N * enu.N + enu.U * enu.U);
return rah;
}
void creatTxt(const std::string& txtPath, const std::string& data)
{
std::ofstream file(txtPath + "/Azimuth.txt");
if (file.is_open())
{
file << data;
file.close();
std::cout << "create txt<78><74>write in Azimuth" << std::endl;
}
else
{
std::cout << "create txt failed!" << std::endl;
}
}
Reference in New Issue View Git Blame Copy Permalink