mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-04-02 23:25:53 +08:00
Changed interpolator (preliminary) to work with PQs and new classes, added stubs for unit tests in BlackCore
This commit is contained in:
Klaus Basan
@@ -1,155 +1,149 @@
|
||||
#include <iostream>
|
||||
#include "blackcore/matrix_3d.h"
|
||||
#include "blackcore/vector_geo.h"
|
||||
#include "blackcore/mathematics.h"
|
||||
/* Copyright (C) 2013 VATSIM Community / contributors
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
|
||||
|
||||
#include "blackcore/interpolator.h"
|
||||
#include "blackcore/constants.h"
|
||||
#include <iostream>
|
||||
|
||||
using namespace BlackMisc::Geo;
|
||||
using namespace BlackMisc::Math;
|
||||
using namespace BlackMisc::PhysicalQuantities;
|
||||
using namespace BlackMisc::Aviation;
|
||||
|
||||
namespace BlackCore
|
||||
{
|
||||
|
||||
CInterpolator::CInterpolator()
|
||||
: m_state_begin(NULL), m_state_end(NULL)
|
||||
/*
|
||||
* Constructor
|
||||
*/
|
||||
CInterpolator::CInterpolator() : m_state_begin(nullptr), m_state_end(nullptr)
|
||||
{
|
||||
m_time.start();
|
||||
}
|
||||
|
||||
/*
|
||||
* Virtual destructor
|
||||
*/
|
||||
CInterpolator::~CInterpolator()
|
||||
{
|
||||
delete m_state_begin;
|
||||
delete m_state_end;
|
||||
delete m_state_begin;
|
||||
delete m_state_end;
|
||||
}
|
||||
|
||||
void CInterpolator::initialize()
|
||||
/*
|
||||
* Initialize
|
||||
*/
|
||||
void CInterpolator::initialize() {}
|
||||
|
||||
/*
|
||||
* Push an update
|
||||
*/
|
||||
CCoordinateNed CInterpolator::pushUpdate(const CCoordinateGeodetic &pos, const CSpeed &groundSpeed, const CHeading &heading, const CAngle &pitch, const CAngle &bank)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void CInterpolator::pushUpdate(CVectorGeo pos, double groundVelocity, double heading, double pitch, double bank)
|
||||
{
|
||||
CNed vNED;
|
||||
|
||||
if ( m_state_begin == NULL )
|
||||
CCoordinateNed velocityNED;
|
||||
if (m_state_begin == nullptr)
|
||||
{
|
||||
m_state_begin = new TPlaneState();
|
||||
m_state_begin = new TPlaneState();
|
||||
m_state_begin->position = CCoordinateTransformation::toEcef(pos);
|
||||
m_state_begin->orientation.heading = heading;
|
||||
m_state_begin->orientation.pitch = pitch;
|
||||
m_state_begin->orientation.bank = bank;
|
||||
m_state_begin->groundspeed = groundSpeed;
|
||||
|
||||
m_state_begin->position = pos.toCartesian();
|
||||
m_state_begin->orientation.heading = heading*Constants::DegToRad;
|
||||
m_state_begin->orientation.pitch = pitch*Constants::DegToRad;
|
||||
m_state_begin->orientation.bank = bank*Constants::DegToRad;
|
||||
m_state_begin->groundspeed = groundVelocity * Constants::KnotsToMeterPerSecond;
|
||||
velocityNED =
|
||||
CCoordinateNed(pos,
|
||||
cos(m_state_begin->orientation.heading.value(CAngleUnit::rad())) * m_state_begin->groundspeed.value(CSpeedUnit::m_s()),
|
||||
sin(m_state_begin->orientation.heading.value(CAngleUnit::rad())) * m_state_begin->groundspeed.value(CSpeedUnit::m_s()), 0);
|
||||
|
||||
vNED.setNorth( cos(m_state_begin->orientation.heading)*m_state_begin->groundspeed );
|
||||
vNED.setEast( sin(m_state_begin->orientation.heading)*m_state_begin->groundspeed );
|
||||
vNED.setDown(0);
|
||||
vNED.setPosition(pos);
|
||||
|
||||
m_state_begin->velocity = vNED.toECEF();
|
||||
|
||||
|
||||
m_state_begin->timestamp = 0;
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
stateNow(m_state_begin);
|
||||
m_state_begin->velocity = CCoordinateTransformation::toEcef(velocityNED).toMathVector();
|
||||
m_state_begin->timestamp = 0;
|
||||
return velocityNED;
|
||||
}
|
||||
|
||||
if ( m_state_end == NULL )
|
||||
{
|
||||
m_state_end = new TPlaneState();
|
||||
}
|
||||
stateNow(m_state_begin);
|
||||
if (m_state_end == nullptr) m_state_end = new TPlaneState();
|
||||
m_state_end->reset();
|
||||
|
||||
|
||||
m_state_end->timestamp = m_time.elapsed();
|
||||
m_state_end->position = CCoordinateTransformation::toEcef(pos);
|
||||
m_state_end->orientation.heading = CHeading(normalizeRadians(heading), false);
|
||||
m_state_end->orientation.pitch = normalizeRadians(pitch);
|
||||
m_state_end->orientation.bank = normalizeRadians(bank);
|
||||
m_state_end->groundspeed = groundSpeed;
|
||||
|
||||
m_state_end->position = pos.toCartesian();
|
||||
m_state_end->orientation.heading = normalizeRadians(heading*Constants::DegToRad);
|
||||
m_state_end->orientation.pitch = normalizeRadians(pitch*Constants::DegToRad);
|
||||
m_state_end->orientation.bank = normalizeRadians(bank*Constants::DegToRad);
|
||||
m_state_end->groundspeed = groundVelocity*Constants::KnotsToMeterPerSecond;
|
||||
|
||||
vNED.setNorth( cos(m_state_end->orientation.heading)*m_state_end->groundspeed );
|
||||
vNED.setEast( sin(m_state_end->orientation.heading)*m_state_end->groundspeed );
|
||||
vNED.setDown(0);
|
||||
vNED.setPosition(pos);
|
||||
m_state_end->velocity = vNED.toECEF();
|
||||
|
||||
std::cout << " Interpolator End velocity: " << std::endl;
|
||||
vNED.print();
|
||||
std::cout << std::endl;
|
||||
|
||||
velocityNED =
|
||||
CCoordinateNed(pos,
|
||||
cos(m_state_end->orientation.heading.value(CAngleUnit::rad())) * m_state_end->groundspeed.value(CSpeedUnit::m_s()),
|
||||
sin(m_state_end->orientation.heading.value(CAngleUnit::rad())) * m_state_end->groundspeed.value(CSpeedUnit::m_s()), 0);
|
||||
m_state_end->velocity = CCoordinateTransformation::toEcef(velocityNED).toMathVector();
|
||||
m_timeEnd = 5;
|
||||
|
||||
double m_TFpow4 = CMath::cubic(m_timeEnd) * m_timeEnd;
|
||||
double m_TFpow4 = CMath::cubic(m_timeEnd) * m_timeEnd;
|
||||
|
||||
m_a = m_state_begin->velocity * CMath::square(m_timeEnd);
|
||||
m_a += m_state_end->velocity * CMath::square(m_timeEnd);
|
||||
|
||||
m_a += m_state_begin->position * m_timeEnd * 2;
|
||||
m_a -= m_state_end->position * m_timeEnd * 2;
|
||||
m_a += m_state_begin->position.toMathVector() * m_timeEnd * 2;
|
||||
m_a -= m_state_end->position.toMathVector() * m_timeEnd * 2;
|
||||
m_a *= 6;
|
||||
m_a /= m_TFpow4;
|
||||
|
||||
m_b = m_state_begin->velocity * CMath::cubic(m_timeEnd) * (-2) - m_state_end->velocity * CMath::cubic(m_timeEnd);
|
||||
m_b = m_b - m_state_begin->position * CMath::square(m_timeEnd) * 3 + m_state_end->position * CMath::square(m_timeEnd) * 3;
|
||||
m_b = m_b * 2 / ( m_TFpow4 );
|
||||
m_b = m_b - m_state_begin->position.toMathVector() * CMath::square(m_timeEnd) * 3 + m_state_end->position.toMathVector() * CMath::square(m_timeEnd) * 3;
|
||||
m_b = m_b * 2 / (m_TFpow4);
|
||||
return velocityNED;
|
||||
}
|
||||
|
||||
bool CInterpolator::isValid()
|
||||
/*
|
||||
* Valid object?
|
||||
*/
|
||||
bool CInterpolator::isValid() const
|
||||
{
|
||||
return (m_state_begin && m_state_end);
|
||||
}
|
||||
|
||||
/*
|
||||
* Calculate current state
|
||||
*/
|
||||
bool CInterpolator::stateNow(TPlaneState *state)
|
||||
{
|
||||
if ( !isValid() )
|
||||
return false;
|
||||
|
||||
if (!this->isValid()) return false;
|
||||
double time = 5;
|
||||
|
||||
/*!
|
||||
Plane Position
|
||||
*/
|
||||
|
||||
// Plane Position
|
||||
double timePow2 = CMath::square(time);
|
||||
double timePow3 = CMath::cubic(time);
|
||||
|
||||
CEcef pos;
|
||||
pos = m_b*3*timePow2*m_timeEnd + m_a * timePow3 * m_timeEnd - m_b * 3 * time * CMath::square(m_timeEnd) - m_a * time* CMath::cubic(m_timeEnd);
|
||||
pos += m_state_begin->position*(-6)*time + m_state_begin->position*6*m_timeEnd + m_state_end->position*6*time;
|
||||
pos /= 6*m_timeEnd;
|
||||
CCoordinateEcef pos(m_b * 3 * timePow2 * m_timeEnd + m_a * timePow3 * m_timeEnd - m_b * 3 * time * CMath::square(m_timeEnd) - m_a * time * CMath::cubic(m_timeEnd));
|
||||
pos += m_state_begin->position * (-6) * time + m_state_begin->position * 6 * m_timeEnd + m_state_end->position * 6 * time;
|
||||
pos /= 6 * m_timeEnd;
|
||||
|
||||
state->position = pos;
|
||||
|
||||
CEcef vel;
|
||||
vel.zeros();
|
||||
vel = m_a * ( 3 * m_timeEnd * CMath::square(time) - CMath::cubic(m_timeEnd));
|
||||
vel += m_b * ( 6 * m_timeEnd * time - 3 * CMath::square(m_timeEnd)) + (m_state_end->position - m_state_begin->position) * 6;
|
||||
vel /= 6*m_timeEnd;
|
||||
|
||||
state->velocity = vel;
|
||||
state->velNED = vel.toNED(pos.toGeodetic());
|
||||
|
||||
/*!
|
||||
Plane Orientation
|
||||
*/
|
||||
|
||||
double vEast = state->velNED.East();
|
||||
double vNorth = state->velNED.North();
|
||||
double fraction = vNorth / vEast;
|
||||
|
||||
double heading = atan2 (vNorth, vEast);
|
||||
CVector3D vel(m_a * (3 * m_timeEnd * CMath::square(time) - CMath::cubic(m_timeEnd)));
|
||||
vel += m_b * (6 * m_timeEnd * time - 3 * CMath::square(m_timeEnd)) + (m_state_end->position - m_state_begin->position).toMathVector() * 6;
|
||||
vel /= 6 * m_timeEnd;
|
||||
|
||||
state->orientation.heading = heading * Constants::RadToDeg;
|
||||
state->velocity = vel;
|
||||
state->velNED = CCoordinateTransformation::toNed(CCoordinateEcef(vel), CCoordinateTransformation::toGeodetic(pos));
|
||||
|
||||
return true;
|
||||
// Plane Orientation
|
||||
double vEast = state->velNED.east();
|
||||
double vNorth = state->velNED.north();
|
||||
state->orientation.heading = CHeading(atan2(vNorth, vEast), false, CAngleUnit::rad());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
double CInterpolator::normalizeRadians(double radian)
|
||||
/*
|
||||
* Normalize radians, clarify what happens here
|
||||
*/
|
||||
CAngle CInterpolator::normalizeRadians(const CAngle &angle) const
|
||||
{
|
||||
return radian - Constants::TwoPI * floor(0.5 + radian / Constants::TwoPI);
|
||||
double radian = angle.value(CAngleUnit::rad());
|
||||
radian = radian - BlackMisc::Math::TwoPI * floor(0.5 + radian / BlackMisc::Math::TwoPI);
|
||||
return CAngle(radian, CAngleUnit::rad());
|
||||
}
|
||||
|
||||
} // namespace BlackCore
|
||||
} // namespace
|
||||
|
||||
Reference in New Issue
Block a user