Interpolator3D.cc

Go to the documentation of this file.

/*
 * Interpolator3D.cpp
 *
 *  Created on: Apr 17, 2015
 *      Author: leven
 */

#include "Interpolator3D.h"
#include "Flight.h"
#include "Airplane.h"
#include "AirplaneUtil.h"
#include "Ads_bDataBase.h"
#include "det/Detector.h"
#include "AirplaneUtil.h"

#include <sstream>

using namespace RdAirplane;
using namespace utl;
using namespace std;
using namespace det;
using namespace boost;
using namespace ROOT::Math;

Interpolator3D::Interpolator3D(
    const vector<const TimeStamp*>& pTimes,
    const vector<const UTMPoint*>& pCoordinates,
    Flight* pFlight) :
  _flight_(pFlight),
  _earliestTime_(**pTimes.begin()),
  _latesTime_(**pTimes.rbegin()) {
  const vector<double>& times = _timesFromTimeStamps(pTimes);
  vector<double> latitudes;
  vector<double> longitudes;
  vector<double> altitudes;
  _getGeodeticCoordinates(pCoordinates, &latitudes, &longitudes, &altitudes);
  vector<double> xs;
  vector<double> ys;
  vector<double> zs;
  _getCoordinates(pCoordinates, &xs, &ys, &zs);
  _interpolatorX_ = new Interpolator(times, xs);
  _interpolatorY_ = new Interpolator(times, ys);
  _interpolatorZ_ = new Interpolator(times, zs);

}

Point Interpolator3D::getCoordinates(const TimeStamp& pTime) const {
  double theTime = _timeFromTimeStamp(pTime);
  return Point(
      _interpolatorX_->Eval(theTime),
      _interpolatorY_->Eval(theTime),
      _interpolatorZ_->Eval(theTime),
      _coordinateSystem());
}

Vector Interpolator3D::getDerivative(const TimeStamp& pTime) const {
  double theTime = _timeFromTimeStamp(pTime);
  return Vector(
      _interpolatorX_->Deriv(theTime),
      _interpolatorY_->Deriv(theTime),
      _interpolatorZ_->Deriv(theTime),
      _coordinateSystem());
}

bool RdAirplane::Interpolator3D::isTimeInInterpolatedRange(const TimeStamp& pTime) const {
  return pTime > _earliestTime_ && pTime < _latesTime_;
}

string RdAirplane::Interpolator3D::getInformation() const {
  stringstream info;
  info << "Interpolating from: " << (_earliestTime_) << " till " << (_latesTime_);
  return info.str();
}

Interpolator3D::~Interpolator3D() {
  delete _interpolatorX_;
  delete _interpolatorY_;
  delete _interpolatorZ_;
}

CoordinateSystemPtr Interpolator3D::_coordinateSystem() {
  return Detector::GetInstance().GetReferenceCoordinateSystem();
}

double Interpolator3D::_timeFromTimeStamp(const TimeStamp& pTime) {
  return Util::ApproximatedGPSTimeStampFromTimestamp(pTime);
}

vector<double> Interpolator3D::_timesFromTimeStamps(const vector<const TimeStamp*>& pTimes) {
  vector<double> res;
  for(vector<const TimeStamp*>::const_iterator iter = pTimes.begin(); iter!=pTimes.end(); ++iter) {
    res.push_back(_timeFromTimeStamp(**iter));
  }
  return res;
}

void Interpolator3D::_getGeodeticCoordinates(
    const vector<const UTMPoint*>& pCoordinates,
    vector<double>* p_OUT_latitudes,
    vector<double>* p_OUT_longitudes,
    vector<double>* p_OUT_altitudes) {
  for(vector<const UTMPoint*>::const_iterator iter = pCoordinates.begin(); iter!=pCoordinates.end(); ++iter) {
    const UTMPoint* utmPoint = *iter;
    tuple<double, double, double> geodeticCoordinates = utmPoint->GetGeodeticCoordinates();
    p_OUT_latitudes->push_back(geodeticCoordinates.get<0>());
    p_OUT_longitudes->push_back(geodeticCoordinates.get<1>());
    p_OUT_altitudes->push_back(geodeticCoordinates.get<2>());
  }
}

void Interpolator3D::_getCoordinates(
    const vector<const UTMPoint*>& pCoordinates,
    vector<double>* p_OUT_xs,
    vector<double>* p_OUT_ys,
    vector<double>* p_OUT_zs) {
  for(vector<const UTMPoint*>::const_iterator iter = pCoordinates.begin(); iter!=pCoordinates.end(); ++iter) {
    const UTMPoint* utmPoint = *iter;
    const Point& point = utmPoint->GetPoint(_coordinateSystem());
    p_OUT_xs->push_back(point.GetX(_coordinateSystem()));
    p_OUT_ys->push_back(point.GetY(_coordinateSystem()));
    p_OUT_zs->push_back(point.GetZ(_coordinateSystem()));
  }
}

tuple<double,double> Interpolator3D::_verySimpleLinearFit(const vector<double>& pX, const vector<double>& pY) {
  double xB = *pX.begin();
  double xE = *pX.end();
  double xD = xE-xB;
  double yB = *pY.begin();
  double yE = *pY.end();
  double yD = yE-yB;
  double slope = yD/xD;
  return tuple<double, double>(yB-xB*slope,slope);
}