90 lines
2.8 KiB
C++
90 lines
2.8 KiB
C++
|
#include "SateOrbit.h"
|
|||
|
|
//#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 <armadillo>
|
|||
|
|
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
using namespace Eigen;
|
|||
|
|
using namespace arma;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
OrbitPoly::OrbitPoly()
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
OrbitPoly::OrbitPoly(int polynum, Eigen::MatrixX<double> polySatellitePara, double SatelliteModelStartTime)
|
|||
|
|
{
|
|||
|
|
if (polySatellitePara.rows() != polynum||polySatellitePara.cols()!=6) {
|
|||
|
|
throw exception("<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>Ͳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>");
|
|||
|
|
}
|
|||
|
|
this->polySatellitePara = polySatellitePara;
|
|||
|
|
this->SatelliteModelStartTime = SatelliteModelStartTime;
|
|||
|
|
this->polynum = polynum;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
OrbitPoly::~OrbitPoly()
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Eigen::MatrixX<double> OrbitPoly::SatelliteSpacePoint(Eigen::MatrixX<double> satellitetime)
|
|||
|
|
{
|
|||
|
|
Eigen::MatrixX<double> result= SatelliteSpacePoints(satellitetime, this->SatelliteModelStartTime, this->polySatellitePara, this->polynum);
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Eigen::MatrixX<double> OrbitPoly::SatelliteSpacePoint(long double satellitetime) {
|
|||
|
|
|
|||
|
|
if (this->polySatellitePara.rows() != polynum || this->polySatellitePara.cols() != 6) {
|
|||
|
|
throw exception("the size of satellitetime has error!! row: p1,p2,p3,p4 col: x,y,z,vx,vy,vz ");
|
|||
|
|
}
|
|||
|
|
// <20><><EFBFBD>¹<EFBFBD><C2B9><EFBFBD>ʱ<EFBFBD><CAB1>
|
|||
|
|
double satellitetime2 =double( satellitetime - this->SatelliteModelStartTime);
|
|||
|
|
Eigen::MatrixX<double> satetime(1, polynum);
|
|||
|
|
for (int i = 0; i < polynum; i++) {
|
|||
|
|
satetime(0, i) = pow(satellitetime2, i);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD>
|
|||
|
|
Eigen::MatrixX<double> satellitePoints(1, 6);
|
|||
|
|
satellitePoints = satetime * polySatellitePara;
|
|||
|
|
return satellitePoints;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// <20><>ȡָ<C8A1><D6B8>ʱ<EFBFBD>̵<EFBFBD><CCB5><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="satellitetime"><3E><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1></param>
|
|||
|
|
/// <param name="SatelliteModelStartTime">ģ<><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1></param>
|
|||
|
|
/// <param name="polySatellitePara">ģ<>Ͳ<EFBFBD><CDB2><EFBFBD>[x1,y1,z1,vx1,vy1,vz1; x2,y2,z2,vx2,vy2,vz2; ..... ]</param>
|
|||
|
|
/// <param name="polynum">ģ<>Ͳ<EFBFBD><CDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD></param>
|
|||
|
|
/// <returns><3E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD></returns>
|
|||
|
|
Eigen::MatrixX<double> SatelliteSpacePoints(Eigen::MatrixX<double>& satellitetime, double SatelliteModelStartTime, Eigen::MatrixX<double>& polySatellitePara, int polynum)
|
|||
|
|
{
|
|||
|
|
if (satellitetime.cols() != 1) {
|
|||
|
|
throw exception("the size of satellitetime has error!!");
|
|||
|
|
}
|
|||
|
|
if (polySatellitePara.rows() != polynum || polySatellitePara.cols() != 6) {
|
|||
|
|
throw exception("the size of satellitetime has error!! row: p1,p2,p3,p4 col: x,y,z,vx,vy,vz ");
|
|||
|
|
}
|
|||
|
|
// <20><><EFBFBD>¹<EFBFBD><C2B9><EFBFBD>ʱ<EFBFBD><CAB1>
|
|||
|
|
int satellitetime_num = satellitetime.rows();
|
|||
|
|
satellitetime = satellitetime.array() - SatelliteModelStartTime;
|
|||
|
|
Eigen::MatrixX<double> satelliteTime(satellitetime_num, polynum);
|
|||
|
|
for (int i = 0; i < polynum; i++) {
|
|||
|
|
satelliteTime.col(i) = satellitetime.array().pow(i);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD>
|
|||
|
|
Eigen::MatrixX<double> satellitePoints(satellitetime_num, 6);
|
|||
|
|
satellitePoints = satelliteTime * polySatellitePara;
|
|||
|
|
return satellitePoints;
|
|||
|
|
}
|