BaseLibraryCPP/GeoOperator.cpp

#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;
}




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);
	//std::cout << 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);
	//std::cout << R << endl;
	double angle = getAngle(Rsc, slopeVector);
	if (angle >= 180) {
		return 360 - angle;
	}
	else {
		return angle;
	}
}