00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "OrbitHistory.h"
00013 namespace O_SESSAME {
00014
00015 OrbitHistory::OrbitHistory()
00016 {
00017 ResetHistory();
00018 m_OrbitHistory.reserve(HISTORY_RESERVE_SIZE);
00019 m_OrbitInterpolations.reserve(HISTORY_RESERVE_SIZE);
00020 }
00021
00022 void OrbitHistory::AppendHistory(const double &_appendTime, const OrbitState &_appendOrbitState)
00023 {
00024 AppendHistory(ssfTime(_appendTime), _appendOrbitState);
00025 }
00026
00027 void OrbitHistory::AppendHistory(const ssfTime &_appendTime, const OrbitState &_appendOrbitState)
00028 {
00029 History::AppendHistory(_appendTime);
00030 m_OrbitHistory.push_back(_appendOrbitState);
00031 return;
00032 }
00033
00034 void OrbitHistory::AppendHistory(const vector<ssfTime> &_appendTime, const vector<OrbitState> &_appendOrbitState)
00035 {
00036 vector<OrbitState>::const_iterator iter;
00037
00038 vector<ssfTime>::difference_type retDistance = History::AppendHistory(_appendTime);
00039
00040
00041 if(retDistance != 0)
00042 m_OrbitHistory.erase(m_OrbitHistory.end()-retDistance, m_OrbitHistory.end());
00043
00044
00045 for(iter = _appendOrbitState.begin(); iter != _appendOrbitState.end(); ++iter)
00046 m_OrbitHistory.push_back(*iter);
00047
00048 return;
00049 }
00050
00051 void OrbitHistory::ResetHistory()
00052 {
00053 vector<OrbitState> newOrbit(0);
00054 m_OrbitHistory.swap(newOrbit);
00055 History::ResetHistory();
00056 return;
00057 }
00058
00059
00060 Matrix OrbitHistory::GetHistory()
00061 {
00062
00063
00064
00065 int OrbitVectorSize = (m_OrbitHistory.back()).GetState().getIndexBound();
00066 Matrix returnMatrix(m_TimeHistory.size(), 1 + OrbitVectorSize);
00067
00068 for(int ii = 0; ii < m_TimeHistory.size(); ii++)
00069 {
00070 returnMatrix(MatrixIndexBase + ii, MatrixIndexBase) = m_TimeHistory[ii].GetSeconds();
00071 returnMatrix(MatrixIndexBase + ii, _(MatrixIndexBase+1,MatrixIndexBase + OrbitVectorSize)) = ~m_OrbitHistory[ii].GetState();
00072 }
00073 return returnMatrix;
00074 }
00075 OrbitState OrbitHistory::GetState(const ssfTime& _requestedTime)
00076 {
00077 OrbitState tempOrbState;
00078 GetState(_requestedTime, tempOrbState);
00079 return tempOrbState;
00080 }
00081 void OrbitHistory::GetState(const ssfTime& _requestedTime, OrbitState& _returnOrbitState)
00082 {
00083
00084 vector<ssfTime>::difference_type nearestTimeIndex = History::GetState(_requestedTime);
00085
00086 int numDataPoints = 0;
00087 if(m_OriginalInterpolator)
00088 numDataPoints = m_OriginalInterpolator->GetNumberDataPoints();
00089 else
00090 return;
00091 Vector timeVector(numDataPoints);
00092 Matrix dataMatrix(numDataPoints, NUM_POSVEL_ELEMENTS);
00093 Vector tempVector(NUM_POSVEL_ELEMENTS);
00094
00095
00096 if(m_TimeHistory[nearestTimeIndex] != _requestedTime)
00097 {
00098
00099
00100 while(m_OrbitInterpolations.capacity() < nearestTimeIndex)
00101 {
00102 m_OrbitInterpolations.reserve(m_OrbitHistory.capacity() + HISTORY_RESERVE_SIZE);
00103 }
00104
00105 if(!m_OrbitInterpolations[nearestTimeIndex])
00106 {
00107
00108 for(int kk = m_OrbitInterpolations.size(); kk < nearestTimeIndex; ++kk)
00109 m_OrbitInterpolations.push_back(m_OriginalInterpolator->NewPointer());
00110
00111 m_OrbitInterpolations.push_back(m_OriginalInterpolator->NewPointer());
00112 }
00113
00114 if(!m_OrbitInterpolations[nearestTimeIndex]->GetValid())
00115 {
00116 for(int ii = 1; ii <= numDataPoints; ++ii)
00117 {
00118 timeVector(ii) = m_TimeHistory[nearestTimeIndex - (numDataPoints/2-1) + ii].GetSeconds();
00119 dataMatrix(ii,_) = ~m_OrbitHistory[nearestTimeIndex - (numDataPoints/2-1) + ii].GetStateRepresentation()->GetPositionVelocity();
00120 }
00121
00122 m_OrbitInterpolations[nearestTimeIndex]->Interpolate(timeVector, dataMatrix);
00123 }
00124
00125 tempVector = m_OrbitInterpolations[nearestTimeIndex]->Evaluate(_requestedTime.GetSeconds());
00126 _returnOrbitState.SetStateRepresentation(new PositionVelocity);
00127 _returnOrbitState.GetStateRepresentation()->SetPositionVelocity(tempVector);
00128
00129
00130 _returnOrbitState.SetOrbitFrame((m_OrbitHistory.front()).GetOrbitFrame());
00131 }
00132 else
00133 {
00134 _returnOrbitState = m_OrbitHistory[nearestTimeIndex];
00135 }
00136
00137 return;
00138 }
00139 }
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172