87 lines
2.7 KiB
C++
87 lines
2.7 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>
|
|
|
|
using namespace std;
|
|
using namespace Eigen;
|
|
|
|
|
|
|
|
OrbitPoly::OrbitPoly()
|
|
{
|
|
}
|
|
|
|
OrbitPoly::OrbitPoly(int polynum, Eigen::MatrixX<double> polySatellitePara, double SatelliteModelStartTime)
|
|
{
|
|
if (polySatellitePara.rows() != polynum||polySatellitePara.cols()!=6) {
|
|
throw exception("?????????????????");
|
|
}
|
|
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 ");
|
|
}
|
|
// ?????????
|
|
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);
|
|
}
|
|
|
|
// ????
|
|
Eigen::MatrixX<double> satellitePoints(1, 6);
|
|
satellitePoints = satetime * polySatellitePara;
|
|
return satellitePoints;
|
|
}
|
|
|
|
/// <summary>
|
|
/// ????????????????????
|
|
/// </summary>
|
|
/// <param name="satellitetime">???????</param>
|
|
/// <param name="SatelliteModelStartTime">??????????</param>
|
|
/// <param name="polySatellitePara">??????[x1,y1,z1,vx1,vy1,vz1; x2,y2,z2,vx2,vy2,vz2; ..... ]</param>
|
|
/// <param name="polynum">??????????</param>
|
|
/// <returns>????????</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 ");
|
|
}
|
|
// ?????????
|
|
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);
|
|
}
|
|
|
|
// ????
|
|
Eigen::MatrixX<double> satellitePoints(satellitetime_num, 6);
|
|
satellitePoints = satelliteTime * polySatellitePara;
|
|
return satellitePoints;
|
|
}
|