449 lines
11 KiB
C++
449 lines
11 KiB
C++
#include "stdafx.h"
|
||
#include "GeoOperator.h"
|
||
#include <iostream>
|
||
#include <Eigen/Core>
|
||
#include <Eigen/Dense>
|
||
#include <time.h>
|
||
////#include <mkl.h>
|
||
#include <string>
|
||
#include <omp.h>
|
||
#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"
|
||
|
||
|
||
|
||
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
|
||
};
|
||
}
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
void LLA2XYZ(const Landpoint& LLA, Point3& XYZ)
|
||
{
|
||
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 };
|
||
XYZ.x = (N + H) * cosB * cos(L);
|
||
XYZ.y = (N + H) * cosB * sin(L);
|
||
//result.z = (N * (1 - e * e) + H) * sin(B);
|
||
XYZ.z = (N * (1 - 1 / f_inverse) * (1 - 1 / f_inverse) + H) * sinB;
|
||
|
||
}
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
|
||
Landpoint XYZ2LLA(const Landpoint& XYZ) {
|
||
double tmpX = XYZ.lon;//
|
||
double temY = XYZ.lat;//
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
void XYZ2BLH_FixedHeight(double x, double y, double z,double ati, Landpoint& point) {
|
||
const double a = 6378137.0; // WGS84<38><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
const double f = 1.0 / 298.257223563;
|
||
const double e2 = 2 * f - f * f; // <20><>һƫ<D2BB><C6AB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
|
||
|
||
// <20><><EFBFBD>㾭<EFBFBD><E3BEAD>L (<28><><EFBFBD><EFBFBD>)
|
||
const double L_rad = std::atan2(y, x);
|
||
point.lon = L_rad * 180.0 / M_PI; // תΪ<D7AA><CEAA>
|
||
|
||
const double p = std::sqrt(x * x + y * y);
|
||
const double H = ati; // ʹ<><CAB9><EFBFBD><EFBFBD>֪<EFBFBD>߶<EFBFBD>
|
||
|
||
// <20><>ʼγ<CABC>ȹ<EFBFBD><C8B9>㣨<EFBFBD><E3A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֪<EFBFBD>߶<EFBFBD>H<EFBFBD><48>
|
||
double B_rad = std::atan2(z, p * (1 - e2 * (a / (a + H))));
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γ<EFBFBD><CEB3>B<EFBFBD><42><EFBFBD>̶<EFBFBD>H<EFBFBD><48>
|
||
for (int i = 0; i < 10; ++i) { // <20><>֪Hʱ<48><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
const double sin_B = std::sin(B_rad);
|
||
const double N = a / std::sqrt(1 - e2 * sin_B * sin_B);
|
||
const double delta = e2 * N / (N + H); // <20>߶ȹ̶<C8B9>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
const double B_new = std::atan2(z, p * (1 - delta));
|
||
|
||
if (std::abs(B_new - B_rad) < 1e-9) {
|
||
B_rad = B_new;
|
||
break;
|
||
}
|
||
B_rad = B_new;
|
||
}
|
||
|
||
point.lat = B_rad * 180.0 / M_PI; // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
||
|
||
// <20><><EFBFBD>ȷ<EFBFBD>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD> [-180<38><30>, 180<38><30>]
|
||
point.lon = std::fmod(point.lon + 360.0, 360.0);
|
||
if (point.lon > 180.0) point.lon -= 360.0;
|
||
point.ati = ati;
|
||
}
|
||
|
||
|
||
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
|
||
};
|
||
}
|
||
|
||
float cross2d(Point3 a, Point3 b)
|
||
{
|
||
return a.x * b.y - a.y * b.x;
|
||
}
|
||
|
||
Point3 operator -(Point3 a, Point3 b)
|
||
{
|
||
return Point3{ a.x - b.x, a.y - b.y, a.z - b.z };
|
||
}
|
||
|
||
Point3 operator +(Point3 a, Point3 b)
|
||
{
|
||
return Point3{ a.x + b.x, a.y + b.y, a.z + b.z };
|
||
}
|
||
|
||
double operator /(Point3 a, Point3 b)
|
||
{
|
||
return sqrt(pow(a.x, 2) + pow(a.y, 2)) / sqrt(pow(b.x, 2) + pow(b.y, 2));
|
||
}
|
||
|
||
|
||
Landpoint getSlopeVector(const Landpoint& p0, const Landpoint& p1, const Landpoint& p2, const Landpoint& p3, const Landpoint& p4,bool inLBH) {
|
||
Landpoint n0, n1, n2, n3, n4;
|
||
if (inLBH) {
|
||
n0 = LLA2XYZ(p0);
|
||
n1 = LLA2XYZ(p1);
|
||
n2 = LLA2XYZ(p2);
|
||
n3 = LLA2XYZ(p3);
|
||
n4 = LLA2XYZ(p4);
|
||
}
|
||
else {
|
||
n0 = p0;
|
||
n1 = p1;
|
||
n2 = p2;
|
||
n3 = p3;
|
||
n4 = 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
|
||
double a3 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np03.lon,np03.lat,0 });// 01->03
|
||
double a4 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np04.lon,np04.lat,0 });// 01->04
|
||
//qDebug() << 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;
|
||
}
|
||
|
||
|
||
|
||
double distance(const Vector3D& p1, const Vector3D& p2)
|
||
{
|
||
double dx = p1.x - p2.x;
|
||
double dy = p1.y - p2.y;
|
||
double dz = p1.z - p2.z;
|
||
return std::sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
double pointToLineDistance(const Vector3D& point, const Vector3D& linePoint, const Vector3D& lineDirection)
|
||
{
|
||
Vector3D pointToLine = { point.x - linePoint.x, point.y - linePoint.y, point.z - linePoint.z };
|
||
|
||
// 计算点到直线的投影点的位<E79A84><E4BD8D>?
|
||
double t = (pointToLine.x * lineDirection.x + pointToLine.y * lineDirection.y + pointToLine.z * lineDirection.z) /
|
||
(lineDirection.x * lineDirection.x + lineDirection.y * lineDirection.y + lineDirection.z * lineDirection.z);
|
||
|
||
// 计算投影<E68A95><E5BDB1>?
|
||
Vector3D projection = { linePoint.x + t * lineDirection.x, linePoint.y + t * lineDirection.y, linePoint.z + t * lineDirection.z };
|
||
|
||
// 计算点到直线的距<E79A84><E8B79D>?
|
||
return distance(point, projection);
|
||
}
|
||
|
||
Vector3D operator+(const Vector3D& p1, const Vector3D& p2)
|
||
{
|
||
return Vector3D{ p1.x + p2.x,p1.y + p2.y,p1.z + p2.z };
|
||
}
|
||
|
||
Vector3D operator-(const Vector3D& p1, const Vector3D& p2)
|
||
{
|
||
return Vector3D{ p1.x - p2.x,p1.y - p2.y,p1.z - p2.z };
|
||
}
|
||
|
||
bool operator==(const Vector3D& p1, const Vector3D& p2)
|
||
{
|
||
return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
|
||
}
|
||
|
||
Vector3D operator*(const Vector3D& p, double scale)
|
||
{
|
||
return Vector3D{
|
||
p.x * scale,
|
||
p.y * scale,
|
||
p.z * scale
|
||
};
|
||
}
|
||
|
||
Vector3D operator*(double scale, const Vector3D& p)
|
||
{
|
||
return Vector3D{
|
||
p.x * scale,
|
||
p.y * scale,
|
||
p.z * scale
|
||
};
|
||
}
|
||
|
||
double getAngle(const Vector3D& a, const Vector3D& b)
|
||
{
|
||
double c = dot(a, b) / (getlength(a) * getlength(b));
|
||
if (a.x * b.y - a.y * b.x >= 0) {
|
||
return acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
|
||
}
|
||
else {
|
||
return 360 - acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
|
||
}
|
||
}
|
||
|
||
double getCosAngle(const Vector3D& a, const Vector3D& b)
|
||
{
|
||
double c = dot(a, b) / (getlength(a) * getlength(b));
|
||
return acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
|
||
}
|
||
|
||
double dot(const Vector3D& p1, const Vector3D& p2)
|
||
{
|
||
return p1.y * p2.y + p1.x * p2.x + p1.z * p2.z;
|
||
}
|
||
|
||
double getlength(const Vector3D& p1)
|
||
{
|
||
return std::sqrt(std::pow(p1.x, 2) + std::pow(p1.y, 2) + std::pow(p1.z, 2));
|
||
}
|
||
|
||
Vector3D crossProduct(const Vector3D& a, const Vector3D& b)
|
||
{
|
||
return Vector3D{
|
||
a.y * b.z - a.z * b.y,//x
|
||
a.z * b.x - a.x * b.z,//y
|
||
a.x * b.y - a.y * b.x//z
|
||
};
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// n1
|
||
/// n4 n0 n2
|
||
/// n3
|
||
/// </summary>
|
||
Vector3D getSlopeVector(const Vector3D& n0, const Vector3D& n1, const Vector3D& n2, const Vector3D& n3, const Vector3D& n4)
|
||
{
|
||
Vector3D n01 = n1 - n0, n02 = n2 - n0, n03 = n3 - n0, n04 = n4 - n0;
|
||
return (crossProduct(n01, n04) + crossProduct(n04, n03) + crossProduct(n03, n02) + crossProduct(n02, n01)) * 0.25;
|
||
}
|
||
|
||
|
||
SphericalCoordinates cartesianToSpherical(const CartesianCoordinates& cartesian)
|
||
{
|
||
SphericalCoordinates spherical;
|
||
|
||
spherical.r = std::sqrt(cartesian.x * cartesian.x + cartesian.y * cartesian.y + cartesian.z * cartesian.z);
|
||
spherical.theta = std::acos(cartesian.z / spherical.r);
|
||
spherical.phi = std::atan2(cartesian.y, cartesian.x);
|
||
|
||
return spherical;
|
||
}
|
||
|
||
CartesianCoordinates sphericalToCartesian(const SphericalCoordinates& spherical)
|
||
{
|
||
CartesianCoordinates cartesian;
|
||
|
||
cartesian.x = spherical.r * std::sin(spherical.theta) * std::cos(spherical.phi);
|
||
cartesian.y = spherical.r * std::sin(spherical.theta) * std::sin(spherical.phi);
|
||
cartesian.z = spherical.r * std::cos(spherical.theta);
|
||
|
||
return cartesian;
|
||
}
|
||
|
||
|
||
double getlocalIncAngle(Landpoint& satepoint, Landpoint& landpoint, Landpoint& slopeVector) {
|
||
Landpoint Rsc = satepoint - landpoint; // AB=B-A
|
||
//double R = getlength(Rsc);
|
||
//qDebug() << R << endl;
|
||
double angle = getAngle(Rsc, slopeVector);
|
||
if (angle >= 180) {
|
||
return 360 - angle;
|
||
}
|
||
else {
|
||
return angle;
|
||
}
|
||
}
|
||
|
||
|
||
double getlocalIncAngle(Vector3D& satepoint, Vector3D& landpoint, Vector3D& slopeVector){
|
||
Vector3D Rsc = satepoint - landpoint; // AB=B-A
|
||
//double R = getlength(Rsc);
|
||
//qDebug() << R << endl;
|
||
double angle = getAngle(Rsc, slopeVector);
|
||
if (angle >= 180) {
|
||
return 360 - angle;
|
||
}
|
||
else {
|
||
return angle;
|
||
}
|
||
}
|