mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-04-24 06:25:37 +08:00
Removed redundant classes after refactoring and moving classes to BlackMisc. Basically the result of the merge with PQ branch.
This commit is contained in:
Klaus Basan
@@ -46,7 +46,6 @@ equals(WITH_SAMPLES, ON) {
|
|||||||
SUBDIRS += samples/com_client/sample_com_client.pro
|
SUBDIRS += samples/com_client/sample_com_client.pro
|
||||||
SUBDIRS += samples/com_server/sample_com_server.pro
|
SUBDIRS += samples/com_server/sample_com_server.pro
|
||||||
SUBDIRS += samples/config/sample_config.pro
|
SUBDIRS += samples/config/sample_config.pro
|
||||||
SUBDIRS += samples/geodetic2ecef/sample_geodetic2ecef.pro
|
|
||||||
SUBDIRS += samples/interpolator/sample_interpolator.pro
|
SUBDIRS += samples/interpolator/sample_interpolator.pro
|
||||||
SUBDIRS += samples/logging/sample_logging.pro
|
SUBDIRS += samples/logging/sample_logging.pro
|
||||||
SUBDIRS += samples/plugin/sample_plugin.pro
|
SUBDIRS += samples/plugin/sample_plugin.pro
|
||||||
|
|||||||
@@ -1,6 +0,0 @@
|
|||||||
FILE(GLOB SRC *.cpp)
|
|
||||||
|
|
||||||
ADD_EXECUTABLE(sample_geodetic2ecef ${SRC})
|
|
||||||
|
|
||||||
TARGET_LINK_LIBRARIES(sample_geodetic2ecef blackmisc blackcore ${QT_LIBRARIES})
|
|
||||||
SET_TARGET_PROPERTIES(sample_geodetic2ecef PROPERTIES PROJECT_LABEL "Samples - Geodetic to Ecef")
|
|
||||||
@@ -1,53 +0,0 @@
|
|||||||
#include <QCoreApplication>
|
|
||||||
#include <QElapsedTimer>
|
|
||||||
#include "blackmisc/debug.h"
|
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
#include "blackcore/ecef.h"
|
|
||||||
#include "blackcore/vector_geo.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
QCoreApplication a(argc, argv);
|
|
||||||
BlackMisc::CApplicationContext myApplicationContext;
|
|
||||||
|
|
||||||
BlackMisc::IContext::getInstance().setSingleton(new BlackMisc::CDebug());
|
|
||||||
|
|
||||||
QElapsedTimer timer;
|
|
||||||
qint64 duration;
|
|
||||||
|
|
||||||
double lat = 27.999999, lon = 86.999999, h = 8820.999999; // Mt Everest
|
|
||||||
|
|
||||||
BlackCore::CEcef mediumvec;
|
|
||||||
BlackCore::CVectorGeo startVec(lat, lon, h);
|
|
||||||
startVec.print();
|
|
||||||
|
|
||||||
cout << std::endl;
|
|
||||||
cout << std::endl;
|
|
||||||
|
|
||||||
BlackCore::CVectorGeo endVec;
|
|
||||||
|
|
||||||
timer.start();
|
|
||||||
mediumvec = startVec.toCartesian();
|
|
||||||
|
|
||||||
duration = timer.nsecsElapsed();
|
|
||||||
|
|
||||||
mediumvec.print();
|
|
||||||
|
|
||||||
cout << std::endl;
|
|
||||||
cout << std::endl;
|
|
||||||
|
|
||||||
cout << "Needed " << duration << " nanoseconds for the calculation!" << std::endl << std::endl;
|
|
||||||
|
|
||||||
timer.restart();
|
|
||||||
endVec = mediumvec.toGeodetic();
|
|
||||||
duration = timer.nsecsElapsed();
|
|
||||||
|
|
||||||
endVec.print();
|
|
||||||
|
|
||||||
cout << "Needed " << duration << " nanoseconds for the calculation!" << std::endl << std::endl;
|
|
||||||
|
|
||||||
return a.exec();
|
|
||||||
}
|
|
||||||
@@ -1,21 +0,0 @@
|
|||||||
# GUI required for matrix classes
|
|
||||||
QT += core gui
|
|
||||||
|
|
||||||
TARGET = sample_geo2ecef
|
|
||||||
TEMPLATE = app
|
|
||||||
|
|
||||||
CONFIG += console
|
|
||||||
CONFIG -= app_bundle
|
|
||||||
|
|
||||||
DEPENDPATH += . ../../src
|
|
||||||
INCLUDEPATH += . ../../src
|
|
||||||
|
|
||||||
SOURCES += *.cpp
|
|
||||||
LIBS += -L../../lib -lblackcore -lblackmisc
|
|
||||||
|
|
||||||
win32: PRE_TARGETDEPS += ../../lib/blackmisc.lib \
|
|
||||||
../../lib/blackcore.lib
|
|
||||||
else: PRE_TARGETDEPS += ../../lib/libblackmisc.a \
|
|
||||||
../../lib/libblackcore.a
|
|
||||||
|
|
||||||
DESTDIR = ../../bin
|
|
||||||
@@ -1,7 +1,6 @@
|
|||||||
#include <QCoreApplication>
|
#include <QCoreApplication>
|
||||||
#include "blackmisc/debug.h"
|
#include "blackmisc/debug.h"
|
||||||
#include "blackmisc/context.h"
|
#include "blackmisc/context.h"
|
||||||
#include "blackcore/constants.h"
|
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
|
|||||||
@@ -1,6 +0,0 @@
|
|||||||
FILE(GLOB SRC *.cpp)
|
|
||||||
|
|
||||||
ADD_EXECUTABLE(sample_geodetic2ecef ${SRC})
|
|
||||||
|
|
||||||
TARGET_LINK_LIBRARIES(sample_geodetic2ecef blackmisc blackcore ${QT_LIBRARIES})
|
|
||||||
SET_TARGET_PROPERTIES(sample_geodetic2ecef PROPERTIES PROJECT_LABEL "Samples - Geodetic to Ecef")
|
|
||||||
@@ -1,21 +0,0 @@
|
|||||||
# GUI required for matrix classes
|
|
||||||
QT += core gui
|
|
||||||
|
|
||||||
TARGET = sample_geo2ecef
|
|
||||||
TEMPLATE = app
|
|
||||||
|
|
||||||
CONFIG += console
|
|
||||||
CONFIG -= app_bundle
|
|
||||||
|
|
||||||
DEPENDPATH += . ../../src
|
|
||||||
INCLUDEPATH += . ../../src
|
|
||||||
|
|
||||||
SOURCES += *.cpp
|
|
||||||
LIBS += -L../../lib -lblackcore -lblackmisc
|
|
||||||
|
|
||||||
win32: PRE_TARGETDEPS += ../../lib/blackmisc.lib \
|
|
||||||
../../lib/blackcore.lib
|
|
||||||
else: PRE_TARGETDEPS += ../../lib/libblackmisc.a \
|
|
||||||
../../lib/libblackcore.a
|
|
||||||
|
|
||||||
DESTDIR = ../../bin
|
|
||||||
@@ -1,7 +1,6 @@
|
|||||||
#include <QCoreApplication>
|
#include <QCoreApplication>
|
||||||
#include "blackmisc/debug.h"
|
#include "blackmisc/debug.h"
|
||||||
#include "blackmisc/context.h"
|
#include "blackmisc/context.h"
|
||||||
#include "blackcore/constants.h"
|
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
|
|||||||
@@ -1,57 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
|
|
||||||
#ifndef CONSTANTS_H
|
|
||||||
#define CONSTANTS_H
|
|
||||||
|
|
||||||
#include "mathematics.h"
|
|
||||||
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
namespace Constants
|
|
||||||
{
|
|
||||||
//! Conversion from Degree to Radians
|
|
||||||
const double DegToRad = 4.0 * atan(1.0) / 180.0;
|
|
||||||
|
|
||||||
//! Conversion from Radians to Degree
|
|
||||||
const double RadToDeg = 180.0 / (4.0 * atan(1.0));
|
|
||||||
|
|
||||||
//! Mathematical constant Pi
|
|
||||||
const double PI = 4.0 * atan(1.0);
|
|
||||||
|
|
||||||
//! 2 * Pi
|
|
||||||
const double TwoPI = 2.0 * PI;
|
|
||||||
|
|
||||||
//! Conversion from feet to meter
|
|
||||||
const double FeetToMeter = 0.3048;
|
|
||||||
|
|
||||||
//! Conversion from meter to feed
|
|
||||||
const double MeterToFeet = 3.28084;
|
|
||||||
|
|
||||||
//! Conversion from knots to meter/second
|
|
||||||
const double KnotsToMeterPerSecond = 0.5144444444;
|
|
||||||
|
|
||||||
//! Equatorial radius of WGS84 ellipsoid (6378137 m)
|
|
||||||
const double EarthRadiusMeters = 6378137.0;
|
|
||||||
|
|
||||||
//! Flattening of WGS84 ellipsoid (1/298.257223563).
|
|
||||||
const double Flattening = 1/298.257223563;
|
|
||||||
|
|
||||||
//! First eccentricity squared
|
|
||||||
const double e2 = (Flattening * (2 - Flattening));
|
|
||||||
|
|
||||||
//! First eccentricity to the power of four
|
|
||||||
const double e4 = CMath::square(e2);
|
|
||||||
|
|
||||||
const double e2m = CMath::square(1 - Flattening);
|
|
||||||
|
|
||||||
const double e2absolute = abs(e2);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
#endif // CONSTANTS_H
|
|
||||||
@@ -1,250 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
//!
|
|
||||||
//! This file incorporates work covered by the following copyright and
|
|
||||||
//! permission notice:
|
|
||||||
//!
|
|
||||||
//! Copyright (c) Charles Karney (2008-2011) <charles@karney.com> and licensed
|
|
||||||
//! under the MIT/X11 License. For more information, see
|
|
||||||
//! http://geographiclib.sourceforge.net/
|
|
||||||
|
|
||||||
#include "blackcore/matrix_3d.h"
|
|
||||||
#include "blackcore/ned.h"
|
|
||||||
#include "blackcore/ecef.h"
|
|
||||||
#include "blackcore/mathematics.h"
|
|
||||||
#include "blackcore/constants.h"
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
CEcef::CEcef()
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
CEcef::CEcef(double X, double Y, double Z)
|
|
||||||
: CVector3D(X, Y, Z)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
CNed CEcef::toNED(const CVectorGeo &pos)
|
|
||||||
{
|
|
||||||
CNed result;
|
|
||||||
|
|
||||||
double angle = - ( pos.latitudeDegrees() * Constants::DegToRad ) - Constants::PI/2;
|
|
||||||
|
|
||||||
CMatrix3D dcm1;
|
|
||||||
CMatrix3D dcm2;
|
|
||||||
CMatrix3D dcm3;
|
|
||||||
CMatrix3D DCM;
|
|
||||||
dcm1.zeros();
|
|
||||||
dcm2.zeros();
|
|
||||||
dcm3.zeros();
|
|
||||||
DCM.zeros();
|
|
||||||
|
|
||||||
dcm1(0,0) = 1;
|
|
||||||
dcm1(1,1) = 1;
|
|
||||||
dcm1(1,2) = 0;
|
|
||||||
dcm1(2,1) = 0;
|
|
||||||
dcm1(2,2) = 1;
|
|
||||||
|
|
||||||
dcm2(0,0) = cos( angle );
|
|
||||||
dcm2(0,2) = -sin( angle );
|
|
||||||
dcm2(1,1) = 1;
|
|
||||||
dcm2(2,0) = sin( angle );
|
|
||||||
dcm2(2,2) = cos( angle );
|
|
||||||
|
|
||||||
angle = pos.longitudeDegrees() * Constants::DegToRad;
|
|
||||||
|
|
||||||
dcm3(0,0) = cos(angle );
|
|
||||||
dcm3(0,1) = sin(angle );
|
|
||||||
dcm3(1,0) = -sin(angle );
|
|
||||||
dcm3(1,1) = cos(angle );
|
|
||||||
dcm3(2,2) = 1;
|
|
||||||
|
|
||||||
DCM = dcm1 * dcm2 * dcm3;
|
|
||||||
|
|
||||||
CVector3D tempResult = DCM * (*this);
|
|
||||||
|
|
||||||
result.setNorth(tempResult.X());
|
|
||||||
result.setEast(tempResult.Y());
|
|
||||||
result.setDown(0);
|
|
||||||
result.setPosition(pos);
|
|
||||||
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CEcef::operator= (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
v[0] = rhs.X();
|
|
||||||
v[1] = rhs.Y();
|
|
||||||
v[2] = rhs.Z();
|
|
||||||
}
|
|
||||||
|
|
||||||
CVectorGeo CEcef::toGeodetic()
|
|
||||||
{
|
|
||||||
CVectorGeo result;
|
|
||||||
|
|
||||||
double R = CMath::hypot(v[0], v[1]);
|
|
||||||
|
|
||||||
double slam = 0;
|
|
||||||
double clam = 1;
|
|
||||||
|
|
||||||
if (R)
|
|
||||||
{
|
|
||||||
slam = v[1] / R;
|
|
||||||
clam = v[0] / R;
|
|
||||||
}
|
|
||||||
|
|
||||||
//! Calculate the distance to the earth
|
|
||||||
double h = CMath::hypot(R, v[2]);
|
|
||||||
|
|
||||||
double sphi = 0;
|
|
||||||
double cphi = 0;
|
|
||||||
|
|
||||||
double p = CMath::square(R / Constants::EarthRadiusMeters);
|
|
||||||
double q = Constants::e2m * CMath::square(v[2] / Constants::EarthRadiusMeters);
|
|
||||||
double r = (p + q - Constants::e4) / 6.0;
|
|
||||||
|
|
||||||
if ( !(Constants::e4 * q == 0 && r <= 0) )
|
|
||||||
{
|
|
||||||
//! Avoid possible division by zero when r = 0 by multiplying
|
|
||||||
//! equations for s and t by r^3 and r, resp.
|
|
||||||
|
|
||||||
double S = Constants::e4 * p * q / 4; //! S = r^3 * s
|
|
||||||
double r2 = CMath::square(r);
|
|
||||||
double r3 = r * r2;
|
|
||||||
double disc = S * (2 * r3 + S);
|
|
||||||
double u = r;
|
|
||||||
|
|
||||||
if (disc >= 0)
|
|
||||||
{
|
|
||||||
double T3 = S + r3;
|
|
||||||
/*!
|
|
||||||
Pick the sign on the sqrt to maximize abs(T3). This minimizes
|
|
||||||
loss of precision due to cancellation. The result is unchanged
|
|
||||||
because of the way the T is used in definition of u.
|
|
||||||
*/
|
|
||||||
T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
|
|
||||||
|
|
||||||
//!N.B. cubicRootReal always returns the real root. cubicRootReal(-8) = -2.
|
|
||||||
double T = CMath::cubicRootReal(T3);
|
|
||||||
|
|
||||||
//! T can be zero; but then r2 / T -> 0.
|
|
||||||
u += T + (T != 0 ? r2 / T : 0);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
//! T is complex, but the way u is defined the result is real.
|
|
||||||
double ang = atan2(sqrt(-disc), -(S + r3));
|
|
||||||
/*!
|
|
||||||
There are three possible cube roots. We choose the root which
|
|
||||||
avoids cancellation. Note that disc < 0 implies that r < 0.
|
|
||||||
*/
|
|
||||||
u += 2 * r * cos(ang / 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
//! This is garanteed positive
|
|
||||||
double V = sqrt(CMath::square(u) + Constants::e4 * q);
|
|
||||||
|
|
||||||
/*!
|
|
||||||
Avoid loss of accuracy when u < 0. Underflow doesn't occur in
|
|
||||||
e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
|
|
||||||
*/
|
|
||||||
double uv = u < 0 ? Constants::e4 * q / (V - u) : u + V; //! u+v, guaranteed positive
|
|
||||||
|
|
||||||
//! Need to guard against w going negative due to roundoff in uv - q.
|
|
||||||
double w = std::max(double(0), Constants::e2absolute * (uv - q) / (2 * V));
|
|
||||||
|
|
||||||
/*!
|
|
||||||
Rearrange expression for k to avoid loss of accuracy due to
|
|
||||||
subtraction. Division by 0 not possible because uv > 0, w >= 0.
|
|
||||||
*/
|
|
||||||
double k = uv / (sqrt(uv + CMath::square(w)) + w);
|
|
||||||
double k1 = k;
|
|
||||||
double k2 = k + Constants::e2;
|
|
||||||
double d = k1 * R / k2;
|
|
||||||
double H = CMath::hypot((v[2])/k1, R/k2);
|
|
||||||
|
|
||||||
sphi = (v[2] / k1) / H;
|
|
||||||
cphi = (R / k2) / H;
|
|
||||||
|
|
||||||
h = (1 - Constants::e2m/k1) * CMath::hypot(d, v[2]);
|
|
||||||
}
|
|
||||||
else //! e4 * q == 0 && r <= 0
|
|
||||||
{
|
|
||||||
/*!
|
|
||||||
This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
|
|
||||||
(prolate, rotation axis) and the generation of 0/0 in the general
|
|
||||||
formulas for phi and h. using the general formula and division by 0
|
|
||||||
in formula for h. So handle this case by taking the limits:
|
|
||||||
f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
|
|
||||||
f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
|
|
||||||
*/
|
|
||||||
double zz = sqrt((Constants::e4 - p) / Constants::e2m);
|
|
||||||
double xx = sqrt( p );
|
|
||||||
double H = CMath::hypot(zz,xx);
|
|
||||||
sphi = zz / H;
|
|
||||||
cphi = xx / H;
|
|
||||||
if (v[2] < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
|
|
||||||
h = - Constants::EarthRadiusMeters * (Constants::e2m) * H / Constants::e2absolute;
|
|
||||||
}
|
|
||||||
|
|
||||||
double lat = atan2(sphi, cphi) * Constants::RadToDeg;
|
|
||||||
|
|
||||||
//! Negative signs return lon in [-180, 180).
|
|
||||||
double lon = -atan2(-slam, clam) * Constants::RadToDeg;
|
|
||||||
|
|
||||||
result.setLongitudeDegrees(lon);
|
|
||||||
result.setLatitudeDegrees(lat);
|
|
||||||
result.setAltitude(h);
|
|
||||||
|
|
||||||
return result;
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
CVectorGeo result;
|
|
||||||
|
|
||||||
double Xpow2plusYpow2 = CMath::square(v[0])+CMath::square(v[1]);
|
|
||||||
|
|
||||||
if( Xpow2plusYpow2 + CMath::square(v[2]) < 25 ) {
|
|
||||||
|
|
||||||
// This function fails near the geocenter region, so catch that special case here.
|
|
||||||
// Define the innermost sphere of small radius as earth center and return the
|
|
||||||
// coordinates 0/0/-EQURAD. It may be any other place on geoide's surface,
|
|
||||||
// the Northpole, Hawaii or Wentorf. This one was easy to code ;-)
|
|
||||||
|
|
||||||
result.setLongitude( 0.0 );
|
|
||||||
result.setLatitude( 0.0 );
|
|
||||||
result.setAltitude( -Constants::EarthRadius );
|
|
||||||
|
|
||||||
return result;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
double R = CMath::hypot(v[0], v[1]);
|
|
||||||
double p = CMath::square(R / Constants::EarthRadius);
|
|
||||||
double q = CMath::square(v[2] / Constants::EarthRadius)*(1-e2);
|
|
||||||
double r = 1/6.0*(p+q-e4);
|
|
||||||
double s = e4*p*q/(4*CMath::cubic(r));
|
|
||||||
|
|
||||||
if( s >= -2.0 && s <= 0.0 )
|
|
||||||
s = 0.0;
|
|
||||||
|
|
||||||
double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
|
|
||||||
double u = r*(1+t+1/t);
|
|
||||||
double vt = sqrt(u*u+e4*q);
|
|
||||||
double w = e2*(u+vt-q)/(2*vt);
|
|
||||||
double k = sqrt(u+vt+w*w)-w;
|
|
||||||
double D = k*R/(k+e2);
|
|
||||||
|
|
||||||
result.setLongitude(2*atan2(v[1], v[0]+R)*RadToDeg);
|
|
||||||
double hypot = CMath::hypot(D, v[2]); //sqrt(D*D+v[2]*v[2]);
|
|
||||||
result.setLatitude(2*atan2(v[2], D+hypot)*RadToDeg);
|
|
||||||
result.setAltitude((k+e2-1)*hypot/k*M_TO_FT);
|
|
||||||
|
|
||||||
return result;*/
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
@@ -1,42 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
|
|
||||||
#ifndef ECEF_H
|
|
||||||
#define ECEF_H
|
|
||||||
|
|
||||||
#include "vector_3d.h"
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
class CNed;
|
|
||||||
class CVectorGeo;
|
|
||||||
|
|
||||||
class CEcef : public CVector3D
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
CEcef();
|
|
||||||
CEcef(double X, double Y, double Z);
|
|
||||||
|
|
||||||
//! Converts this velocity vector into the NED format
|
|
||||||
/*!
|
|
||||||
\param pos This position is needed for correct calculation
|
|
||||||
\return velocity in NED coordinate system
|
|
||||||
*/
|
|
||||||
CNed toNED(const CVectorGeo &pos);
|
|
||||||
|
|
||||||
//! Converts this position vector into the geodetic format
|
|
||||||
/*!
|
|
||||||
\return Position in latitude, longitude and altitude
|
|
||||||
*/
|
|
||||||
CVectorGeo toGeodetic();
|
|
||||||
|
|
||||||
//! Assignment operator
|
|
||||||
void operator= (const CVector3D &rhs);
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
#endif // ECEF_H
|
|
||||||
@@ -1,33 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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 <algorithm> // std::max
|
|
||||||
|
|
||||||
#include "blackcore/mathematics.h"
|
|
||||||
|
|
||||||
namespace BlackCore {
|
|
||||||
|
|
||||||
double CMath::hypot(double x, double y)
|
|
||||||
{
|
|
||||||
x = abs(x);
|
|
||||||
y = abs(y);
|
|
||||||
|
|
||||||
double max = std::max(x,y);
|
|
||||||
double min = std::min(x,y);
|
|
||||||
|
|
||||||
double r = min/max;
|
|
||||||
|
|
||||||
return max * sqrt(1 + r*r);
|
|
||||||
}
|
|
||||||
|
|
||||||
double CMath::cubicRootReal(const double x)
|
|
||||||
{
|
|
||||||
double result;
|
|
||||||
result = pow(abs(x), (double)1/3);
|
|
||||||
|
|
||||||
return x < 0 ? -result : result;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
|
|
||||||
#ifndef MATH_H
|
|
||||||
#define MATH_H
|
|
||||||
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
class CMath
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
//! Calculates the hypotenuse of x and y without overflow
|
|
||||||
/*!
|
|
||||||
\param x
|
|
||||||
\param y
|
|
||||||
*/
|
|
||||||
static double hypot(double x, double y);
|
|
||||||
|
|
||||||
//! Calculates the square of x
|
|
||||||
/*!
|
|
||||||
\param x
|
|
||||||
\return x^2
|
|
||||||
*/
|
|
||||||
static inline double square(const double x)
|
|
||||||
{
|
|
||||||
return x*x;
|
|
||||||
}
|
|
||||||
|
|
||||||
//! Calculates x to the power of three
|
|
||||||
/*!
|
|
||||||
\param x
|
|
||||||
\return x^3
|
|
||||||
*/
|
|
||||||
static inline double cubic(const double x)
|
|
||||||
{
|
|
||||||
return x*x*x;
|
|
||||||
}
|
|
||||||
|
|
||||||
//! Calculates the real cubic root
|
|
||||||
/*!
|
|
||||||
\param x
|
|
||||||
\param y
|
|
||||||
\return Returns the real part of the solution
|
|
||||||
*/
|
|
||||||
static double cubicRootReal(const double x);
|
|
||||||
|
|
||||||
private:
|
|
||||||
CMath() {}
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
#endif // MATH_H
|
|
||||||
@@ -1,255 +0,0 @@
|
|||||||
#include "blackcore/matrix_3d.h"
|
|
||||||
#include "blackcore/vector_3d.h"
|
|
||||||
#include "blackmisc/debug.h"
|
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
/* TODO
|
|
||||||
*
|
|
||||||
class Vector2D
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
Vector data;//der macht copy-ctor, dtor und op=
|
|
||||||
public:
|
|
||||||
class Zeilenproxy
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
Vector2D& v2d;
|
|
||||||
int zeile;
|
|
||||||
public:
|
|
||||||
Zeilenproxy(Vector2D& _v2d,int _y)
|
|
||||||
:v2d(_v2d),y(_y)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
double& operator[](int x)
|
|
||||||
{
|
|
||||||
return v2d.data[y*SIZEX+x];//kacke, nur demo
|
|
||||||
}
|
|
||||||
}
|
|
||||||
ZeilenProxy operator[](int y)
|
|
||||||
{
|
|
||||||
return Zeilenproxy(*this,y);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
*/
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
CMatrix3D::CMatrix3D()
|
|
||||||
{
|
|
||||||
zeros();
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D::CMatrix3D(const CMatrix3D & other)
|
|
||||||
{
|
|
||||||
zeros();
|
|
||||||
for(int i=0; i<3; i++)
|
|
||||||
{
|
|
||||||
for(int j=0; j<3; j++)
|
|
||||||
{
|
|
||||||
m[i][j] = other.getElement(i, j);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void CMatrix3D::random()
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
double CMatrix3D::determinant()
|
|
||||||
{
|
|
||||||
double determinant = m[0][0]*m[1][1]*m[2][2] +
|
|
||||||
m[0][1]*m[1][2]*m[2][0] +
|
|
||||||
m[0][2]*m[1][0]*m[2][1] -
|
|
||||||
m[0][1]*m[1][0]*m[2][2] -
|
|
||||||
m[0][2]*m[1][1]*m[2][0] -
|
|
||||||
m[0][0]*m[1][2]*m[2][1];
|
|
||||||
|
|
||||||
return determinant;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D CMatrix3D::inverse()
|
|
||||||
{
|
|
||||||
CMatrix3D result;
|
|
||||||
double det = determinant();
|
|
||||||
|
|
||||||
if (!det)
|
|
||||||
return result;
|
|
||||||
|
|
||||||
double invdet = 1 / determinant();
|
|
||||||
|
|
||||||
result.m[0][0] = ( m[1][1]*m[2][2] - m[1][2]*m[2][1] ) * invdet;
|
|
||||||
result.m[0][1] = ( - m[0][1]*m[2][2] + m[0][2]*m[2][1] ) * invdet;
|
|
||||||
result.m[0][2] = ( m[0][1]*m[1][2] - m[0][2]*m[1][1] ) * invdet;
|
|
||||||
result.m[1][0] = ( - m[1][0]*m[2][2] + m[1][2]*m[2][0] ) * invdet;
|
|
||||||
result.m[1][1] = ( m[0][0]*m[2][2] - m[0][2]*m[2][0] ) * invdet;
|
|
||||||
result.m[1][2] = ( - m[0][0]*m[1][2] + m[0][2]*m[1][0] ) * invdet;
|
|
||||||
result.m[2][0] = ( m[1][0]*m[2][1] - m[1][1]*m[2][0] ) * invdet;
|
|
||||||
result.m[2][1] = ( - m[0][0]*m[2][1] + m[0][1]*m[2][0] ) * invdet;
|
|
||||||
result.m[2][2] = ( m[0][0]*m[1][1] - m[0][1]*m[1][0] ) * invdet;
|
|
||||||
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CMatrix3D::zeros()
|
|
||||||
{
|
|
||||||
for(int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
for(int j=0; j<3; ++j)
|
|
||||||
{
|
|
||||||
m[i][j] = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void CMatrix3D::print()
|
|
||||||
{
|
|
||||||
std::cout << m[0][0] << " " << m[0][1] << " " << m[0][2] << std::endl;
|
|
||||||
std::cout << m[1][0] << " " << m[1][1] << " " << m[1][2] << std::endl;
|
|
||||||
std::cout << m[2][0] << " " << m[2][1] << " " << m[2][2] << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
double CMatrix3D::getElement(qint8 row, qint8 column) const
|
|
||||||
{
|
|
||||||
Q_ASSERT (row < 3 || column < 3);
|
|
||||||
|
|
||||||
return m[row][column];
|
|
||||||
}
|
|
||||||
|
|
||||||
void CMatrix3D::setElement(qint8 row, qint8 column, double value)
|
|
||||||
{
|
|
||||||
Q_ASSERT (row < 3 || column < 3);
|
|
||||||
|
|
||||||
m[row][column] = value;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D & CMatrix3D::operator +=(const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
for(int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
for(int j=0; j<3; ++j)
|
|
||||||
{
|
|
||||||
m[i][j] += rhs.getElement(i, j);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D & CMatrix3D::operator -=(const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
for(int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
for(int j=0; j<3; ++j)
|
|
||||||
{
|
|
||||||
m[i][j] -= rhs.getElement(i, j);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D& CMatrix3D::operator = (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
if (this != &rhs)
|
|
||||||
{
|
|
||||||
for(int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
for(int j=0; j<3; ++j)
|
|
||||||
{
|
|
||||||
m[i][j] = rhs.getElement(i, j);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D CMatrix3D::operator + (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
CMatrix3D helper = *this;
|
|
||||||
helper += rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D CMatrix3D::operator - (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
CMatrix3D helper = *this;
|
|
||||||
helper -= rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CMatrix3D::operator == (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
bool isEqual = true;
|
|
||||||
for(int i=0; i<3 && isEqual; ++i)
|
|
||||||
{
|
|
||||||
for(int j=0; j<3 && isEqual; ++j)
|
|
||||||
{
|
|
||||||
isEqual = (m[i][j] == rhs.getElement(i, j));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return isEqual;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CMatrix3D::operator != (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
return !(*this == rhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D & CMatrix3D::operator *= (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
CMatrix3D helper(*this);
|
|
||||||
for(qint32 column = 0; column < 3; ++column)
|
|
||||||
{
|
|
||||||
for(qint32 row = 0; row < 3; ++row )
|
|
||||||
{
|
|
||||||
m[row][column] = helper.getElement(row,0) * rhs.getElement(0, column);
|
|
||||||
for(qint32 line = 1; line < 3; ++line)
|
|
||||||
{
|
|
||||||
m[row][column] += helper.getElement(row,line) * rhs.getElement(line, column);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CMatrix3D CMatrix3D::operator * (const CMatrix3D &rhs)
|
|
||||||
{
|
|
||||||
CMatrix3D helper(*this);
|
|
||||||
helper *= rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D CMatrix3D::operator * ( const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
CVector3D result;
|
|
||||||
for (qint32 i = 0; i < 3; ++i)
|
|
||||||
{
|
|
||||||
for (qint32 j = 0; j < 3; ++j)
|
|
||||||
{
|
|
||||||
result.v[i] += m[i][j] * rhs.v[j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
CEcef CMatrix3D::operator * ( const CEcef &rhs)
|
|
||||||
{
|
|
||||||
CEcef result;
|
|
||||||
for (qint32 i = 0; i < 3; ++i)
|
|
||||||
{
|
|
||||||
for (qint32 j = 0; j < 3; ++j)
|
|
||||||
{
|
|
||||||
result.v[i] += m[i][j] * rhs.v[j];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
double& CMatrix3D::operator() (const qint8 row, const qint8 column)
|
|
||||||
{
|
|
||||||
return m[row][column];
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
@@ -1,86 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
|
|
||||||
#ifndef MATRIX_3D_H
|
|
||||||
#define MATRIX_3D_H
|
|
||||||
|
|
||||||
#include "blackcore/vector_3d.h"
|
|
||||||
#include "blackcore/ecef.h"
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
class CMatrix3D
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
CMatrix3D();
|
|
||||||
|
|
||||||
CMatrix3D(const CMatrix3D & other);
|
|
||||||
|
|
||||||
/*!
|
|
||||||
Basic Matrix functions
|
|
||||||
*/
|
|
||||||
|
|
||||||
//! Fills the matrix with random elements
|
|
||||||
void random();
|
|
||||||
|
|
||||||
//! Calculates the determinant of the matrix
|
|
||||||
double determinant();
|
|
||||||
|
|
||||||
//! Returns the inverse matrix
|
|
||||||
CMatrix3D inverse();
|
|
||||||
|
|
||||||
//! Sets all elements to zero
|
|
||||||
void zeros();
|
|
||||||
|
|
||||||
//! Prints the matrix to stdout
|
|
||||||
void print();
|
|
||||||
|
|
||||||
//! Returns an element
|
|
||||||
/*!
|
|
||||||
\param row Specifies elements row
|
|
||||||
\param column Specifies elements column
|
|
||||||
\return Returns element of [row, column]
|
|
||||||
*/
|
|
||||||
double getElement(qint8 row, qint8 column) const;
|
|
||||||
|
|
||||||
//! Sets a matrix element
|
|
||||||
/*!
|
|
||||||
\param row Specifies elements row
|
|
||||||
\param column Specifies elements column
|
|
||||||
\param value Specifies the new elements value
|
|
||||||
*/
|
|
||||||
void setElement(qint8 row, qint8 column, double value);
|
|
||||||
|
|
||||||
/*!
|
|
||||||
Operators
|
|
||||||
*/
|
|
||||||
|
|
||||||
CMatrix3D & operator +=(const CMatrix3D &rhs);
|
|
||||||
CMatrix3D & operator -=(const CMatrix3D &rhs);
|
|
||||||
|
|
||||||
CMatrix3D & operator = (const CMatrix3D &rhs);
|
|
||||||
|
|
||||||
CMatrix3D operator +(const CMatrix3D &rhs);
|
|
||||||
CMatrix3D operator -(const CMatrix3D &rhs);
|
|
||||||
|
|
||||||
bool operator ==(const CMatrix3D &rhs);
|
|
||||||
bool operator !=(const CMatrix3D &rhs);
|
|
||||||
|
|
||||||
CMatrix3D & operator *=(const CMatrix3D &rhs);
|
|
||||||
CMatrix3D operator *(const CMatrix3D &rhs);
|
|
||||||
|
|
||||||
CVector3D operator * ( const CVector3D &rhs);
|
|
||||||
CEcef operator * ( const CEcef &rhs);
|
|
||||||
|
|
||||||
double& operator() (const qint8 row, const qint8 column);
|
|
||||||
|
|
||||||
private:
|
|
||||||
double m[3][3];
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
#endif // MATRIX_3D_H
|
|
||||||
@@ -1,74 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/matrix_3d.h"
|
|
||||||
#include "blackcore/ecef.h"
|
|
||||||
#include "blackcore/ned.h"
|
|
||||||
|
|
||||||
#include "blackcore/constants.h"
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
CNed::CNed()
|
|
||||||
{
|
|
||||||
zeros();
|
|
||||||
}
|
|
||||||
|
|
||||||
CNed::CNed(CVectorGeo &pos, double N, double E, double D)
|
|
||||||
: CVector3D(N, E, D), m_position(pos)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
CEcef CNed::toECEF()
|
|
||||||
{
|
|
||||||
double angle = - ( m_position.latitudeDegrees() * Constants::DegToRad ) - Constants::PI/2;
|
|
||||||
|
|
||||||
CMatrix3D dcm1;
|
|
||||||
CMatrix3D dcm2;
|
|
||||||
CMatrix3D dcm3;
|
|
||||||
CMatrix3D DCM;
|
|
||||||
CMatrix3D invDCM;
|
|
||||||
dcm1.zeros();
|
|
||||||
dcm2.zeros();
|
|
||||||
dcm3.zeros();
|
|
||||||
|
|
||||||
dcm1(0,0) = 1;
|
|
||||||
dcm1(1,1) = 1;
|
|
||||||
dcm1(1,2) = 0;
|
|
||||||
dcm1(2,1) = 0;
|
|
||||||
dcm1(2,2) = 1;
|
|
||||||
|
|
||||||
dcm2(0,0) = cos( angle );
|
|
||||||
dcm2(0,2) = -sin( angle );
|
|
||||||
dcm2(1,1) = 1;
|
|
||||||
dcm2(2,0) = sin( angle );
|
|
||||||
dcm2(2,2) = cos( angle );
|
|
||||||
|
|
||||||
angle = m_position.longitudeDegrees() * Constants::DegToRad;
|
|
||||||
|
|
||||||
dcm3(0,0) = cos(angle );
|
|
||||||
dcm3(0,1) = sin(angle );
|
|
||||||
dcm3(1,0) = -sin(angle );
|
|
||||||
dcm3(1,1) = cos(angle );
|
|
||||||
dcm3(2,2) = 1;
|
|
||||||
|
|
||||||
DCM = dcm1 * dcm2 * dcm3;
|
|
||||||
|
|
||||||
invDCM.zeros();
|
|
||||||
invDCM = DCM.inverse();
|
|
||||||
|
|
||||||
CVector3D tempResult = invDCM * (*this);
|
|
||||||
CEcef result;
|
|
||||||
|
|
||||||
result.setX(tempResult.X());
|
|
||||||
result.setY(tempResult.Y());
|
|
||||||
result.setZ(tempResult.Z());
|
|
||||||
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
@@ -1,47 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
|
|
||||||
#ifndef VECTOR_NED_H
|
|
||||||
#define VECTOR_NED_H
|
|
||||||
|
|
||||||
#include "vector_3d.h"
|
|
||||||
#include "vector_geo.h"
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
class CEcef;
|
|
||||||
|
|
||||||
class CNed : public CVector3D
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
CNed();
|
|
||||||
CNed(CVectorGeo &pos, double N, double E, double D);
|
|
||||||
|
|
||||||
double North() const {return v[0];}
|
|
||||||
|
|
||||||
double East() const {return v[1];}
|
|
||||||
|
|
||||||
double Down() const {return v[2];}
|
|
||||||
|
|
||||||
CVectorGeo position() const { return m_position; }
|
|
||||||
|
|
||||||
void setNorth(const double num) { v[0] = num; }
|
|
||||||
|
|
||||||
void setEast(const double num) { v[1] = num; }
|
|
||||||
|
|
||||||
void setDown(const double num) { v[2] = num; }
|
|
||||||
|
|
||||||
void setPosition(const CVectorGeo &pos ) { m_position = pos; }
|
|
||||||
|
|
||||||
CEcef toECEF();
|
|
||||||
|
|
||||||
private:
|
|
||||||
CVectorGeo m_position;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
#endif // VECTOR_NED_H
|
|
||||||
@@ -7,9 +7,8 @@
|
|||||||
#define SIMULATOR_H
|
#define SIMULATOR_H
|
||||||
|
|
||||||
#include "blackcore/sim_callbacks.h"
|
#include "blackcore/sim_callbacks.h"
|
||||||
#include "blackcore/vector_geo.h"
|
#include "blackmisc/coordinategeodetic.h"
|
||||||
#include "blackcore/vector_3d.h"
|
#include "blackmisc/mathvector3d.h"
|
||||||
|
|
||||||
#include <QString>
|
#include <QString>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
|
||||||
@@ -21,13 +20,15 @@
|
|||||||
* \file Simulator driver interface.
|
* \file Simulator driver interface.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
namespace BlackMisc {
|
namespace BlackMisc
|
||||||
|
{
|
||||||
|
|
||||||
class IContext;
|
class IContext;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace BlackCore {
|
namespace BlackCore
|
||||||
|
{
|
||||||
|
|
||||||
/*!
|
/*!
|
||||||
* A struct to hold data about an aircraft model and repaint.
|
* A struct to hold data about an aircraft model and repaint.
|
||||||
@@ -47,14 +48,13 @@ namespace BlackCore {
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
//! Constructor, initialize to zero.
|
//! Constructor, initialize to zero.
|
||||||
CPhysicalState() : headingDegrees(0), pitchDegrees(0), bankDegrees(0), groundSpeedKnots(0)
|
CPhysicalState() : headingDegrees(0), pitchDegrees(0), bankDegrees(0), groundSpeedKnots(0) {}
|
||||||
{}
|
BlackMisc::Geo::CCoordinateGeodetic position; //!< geographical position
|
||||||
CVectorGeo position; //!< geographical position
|
|
||||||
float headingDegrees; //!< heading in degrees
|
float headingDegrees; //!< heading in degrees
|
||||||
float pitchDegrees; //!< pitch in degrees
|
float pitchDegrees; //!< pitch in degrees
|
||||||
float bankDegrees; //!< bank in degrees
|
float bankDegrees; //!< bank in degrees
|
||||||
float groundSpeedKnots; //!< ground speed in knots
|
float groundSpeedKnots; //!< ground speed in knots
|
||||||
CVector3D trueSpeedMetersPerSec; //!< needed by FSX
|
BlackMisc::Math::CVector3D trueSpeedMetersPerSec; //!< needed by FSX
|
||||||
};
|
};
|
||||||
|
|
||||||
/*!
|
/*!
|
||||||
@@ -134,10 +134,11 @@ namespace BlackCore {
|
|||||||
static const quint32 InterfaceVersionMinor;
|
static const quint32 InterfaceVersionMinor;
|
||||||
|
|
||||||
//! Enumeration to describe which simulator is desired.
|
//! Enumeration to describe which simulator is desired.
|
||||||
enum ESimulator {
|
enum ESimulator
|
||||||
|
{
|
||||||
FS9 = 0, //!< Microsoft Flight Simulator 9
|
FS9 = 0, //!< Microsoft Flight Simulator 9
|
||||||
FSX, //!< Microsoft Flight Simulator 10
|
FSX, //!< Microsoft Flight Simulator 10
|
||||||
XPLANE, //!< X-Plane
|
XPLANE //!< X-Plane
|
||||||
};
|
};
|
||||||
|
|
||||||
//! Constructor.
|
//! Constructor.
|
||||||
|
|||||||
@@ -1,175 +0,0 @@
|
|||||||
|
|
||||||
#include "blackcore/vector_3d.h"
|
|
||||||
#include "blackcore/matrix_3d.h"
|
|
||||||
#include "blackcore/vector_geo.h"
|
|
||||||
#include "blackcore/constants.h"
|
|
||||||
#include "blackmisc/debug.h"
|
|
||||||
#include <iostream>
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
CVector3D::CVector3D()
|
|
||||||
{
|
|
||||||
zeros();
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D::CVector3D(double x, double y, double z)
|
|
||||||
{
|
|
||||||
zeros();
|
|
||||||
v[0] = x;
|
|
||||||
v[1] = y;
|
|
||||||
v[2] = z;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D::CVector3D(const CVector3D &other)
|
|
||||||
{
|
|
||||||
zeros();
|
|
||||||
for (int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
v[i] = other.getElement(i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void CVector3D::print()
|
|
||||||
{
|
|
||||||
std::cout << "v = " << std::endl;
|
|
||||||
std::cout << std::fixed;
|
|
||||||
for (qint32 i = 0; i < 3; ++i)
|
|
||||||
{
|
|
||||||
std::cout << "[" << v[i] << "]" << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void CVector3D::zeros()
|
|
||||||
{
|
|
||||||
for (qint32 i = 0; i < 3; ++i)
|
|
||||||
{
|
|
||||||
v[i] = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
double CVector3D::magnitude()
|
|
||||||
{
|
|
||||||
return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
|
|
||||||
}
|
|
||||||
|
|
||||||
double CVector3D::getElement(qint8 row) const
|
|
||||||
{
|
|
||||||
Q_ASSERT(row < 3);
|
|
||||||
|
|
||||||
return v[row];
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D &CVector3D::operator +=(const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
for (int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
v[i] += rhs.getElement(i);
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D &CVector3D::operator -=(const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
for (int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
v[i] -= rhs.getElement(i);
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D &CVector3D::operator =(const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
if (this != &rhs)
|
|
||||||
{
|
|
||||||
for (int i=0; i<3; ++i)
|
|
||||||
{
|
|
||||||
v[i] = rhs.getElement(i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D CVector3D::operator + (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper = *this;
|
|
||||||
helper += rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D CVector3D::operator - (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper = *this;
|
|
||||||
helper -= rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CVector3D::operator == (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
bool isEqual = true;
|
|
||||||
for(int i=0; i<3 && isEqual; ++i)
|
|
||||||
{
|
|
||||||
isEqual = (v[i] == rhs.getElement(i));
|
|
||||||
}
|
|
||||||
return isEqual;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CVector3D::operator != (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
return !(*this == rhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D & CVector3D::operator *= (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper(*this);
|
|
||||||
for(qint32 row = 0; row < 3; ++row )
|
|
||||||
{
|
|
||||||
v[row] = helper.getElement(row) * rhs.getElement(row);
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D CVector3D::operator * (const CVector3D &rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper(*this);
|
|
||||||
helper *= rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D &CVector3D::operator *= ( const double rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper(*this);
|
|
||||||
for(qint32 row = 0; row < 3; ++row )
|
|
||||||
{
|
|
||||||
v[row] = helper.getElement(row) * rhs;
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D CVector3D::operator * (const double rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper(*this);
|
|
||||||
helper *= rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D &CVector3D::operator /= ( const double rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper(*this);
|
|
||||||
for(qint32 row = 0; row < 3; ++row )
|
|
||||||
{
|
|
||||||
v[row] = helper.getElement(row) / rhs;
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVector3D CVector3D::operator / (const double rhs)
|
|
||||||
{
|
|
||||||
CVector3D helper(*this);
|
|
||||||
helper /= rhs;
|
|
||||||
return helper;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
@@ -1,73 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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 <qglobal.h>
|
|
||||||
|
|
||||||
#ifndef VECTOR_3D_H
|
|
||||||
#define VECTOR_3D_H
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
class CMatrix3D;
|
|
||||||
|
|
||||||
class CVector3D
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
CVector3D();
|
|
||||||
CVector3D(double x, double y, double z);
|
|
||||||
CVector3D( const CVector3D & other);
|
|
||||||
|
|
||||||
double X() const {return v[0];}
|
|
||||||
|
|
||||||
double Y() const {return v[1];}
|
|
||||||
|
|
||||||
double Z() const {return v[2];}
|
|
||||||
|
|
||||||
void setX(const double num) { v[0] = num; }
|
|
||||||
|
|
||||||
void setY(const double num) { v[1] = num; }
|
|
||||||
|
|
||||||
void setZ(const double num) { v[2] = num; }
|
|
||||||
|
|
||||||
double getElement(qint8 row) const;
|
|
||||||
|
|
||||||
void print();
|
|
||||||
|
|
||||||
void zeros();
|
|
||||||
|
|
||||||
CVector3D & operator +=(const CVector3D &rhs);
|
|
||||||
CVector3D & operator -=(const CVector3D &rhs);
|
|
||||||
CVector3D & operator = (const CVector3D &rhs);
|
|
||||||
|
|
||||||
CVector3D operator +(const CVector3D &rhs);
|
|
||||||
CVector3D operator -(const CVector3D &rhs);
|
|
||||||
|
|
||||||
bool operator ==(const CVector3D &rhs);
|
|
||||||
bool operator !=(const CVector3D &rhs);
|
|
||||||
|
|
||||||
//double crossProduct(qint32 );
|
|
||||||
|
|
||||||
|
|
||||||
CVector3D & operator *=(const CVector3D &rhs);
|
|
||||||
CVector3D operator *( const CVector3D &rhs);
|
|
||||||
|
|
||||||
CVector3D & operator *=( const double rhs);
|
|
||||||
CVector3D operator *( const double rhs);
|
|
||||||
|
|
||||||
CVector3D & operator /=( const double rhs);
|
|
||||||
CVector3D operator /( const double rhs);
|
|
||||||
|
|
||||||
double magnitude();
|
|
||||||
|
|
||||||
protected:
|
|
||||||
double v[3];
|
|
||||||
|
|
||||||
friend class CMatrix3D;
|
|
||||||
};
|
|
||||||
|
|
||||||
} //! namespace BlackCore
|
|
||||||
|
|
||||||
#endif // VECTOR_3D_H
|
|
||||||
@@ -1,86 +0,0 @@
|
|||||||
#include <math.h>
|
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
#include "blackcore/constants.h"
|
|
||||||
#include "blackcore/vector_geo.h"
|
|
||||||
#include "blackcore/ecef.h"
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
CVectorGeo::CVectorGeo()
|
|
||||||
: m_latitudeDegrees(0), m_longitudeDegrees(0), m_altitudeMeters(0)
|
|
||||||
{}
|
|
||||||
|
|
||||||
CVectorGeo::CVectorGeo(double latitudeDegrees, double longitudeDegrees, double altitudeMeters)
|
|
||||||
: m_latitudeDegrees(latitudeDegrees), m_longitudeDegrees(longitudeDegrees),
|
|
||||||
m_altitudeMeters(altitudeMeters)
|
|
||||||
{}
|
|
||||||
|
|
||||||
CVectorGeo::CVectorGeo(const CVectorGeo &other)
|
|
||||||
: m_latitudeDegrees(other.m_latitudeDegrees), m_longitudeDegrees(other.m_longitudeDegrees),
|
|
||||||
m_altitudeMeters(other.m_altitudeMeters)
|
|
||||||
{}
|
|
||||||
|
|
||||||
CEcef CVectorGeo::toCartesian()
|
|
||||||
{
|
|
||||||
CEcef result;
|
|
||||||
|
|
||||||
double phi = m_latitudeDegrees * Constants::DegToRad;
|
|
||||||
double lambda = m_longitudeDegrees * Constants::DegToRad;
|
|
||||||
double sphi = sin(phi);
|
|
||||||
double cphi = 0;
|
|
||||||
if (std::abs(m_latitudeDegrees) != 90)
|
|
||||||
cphi = cos(phi);
|
|
||||||
|
|
||||||
double n = Constants::EarthRadiusMeters/sqrt(1-Constants::e2 * CMath::square(sphi));
|
|
||||||
|
|
||||||
double slambda = 0;
|
|
||||||
if (m_longitudeDegrees != -180)
|
|
||||||
slambda = sin(lambda);
|
|
||||||
|
|
||||||
double clambda = 0;
|
|
||||||
if (std::abs(m_longitudeDegrees) != 90)
|
|
||||||
clambda = cos(lambda);
|
|
||||||
|
|
||||||
double h = m_altitudeMeters;
|
|
||||||
|
|
||||||
double X = (n + h) * cphi;
|
|
||||||
double Y = X * slambda;
|
|
||||||
X *= clambda;
|
|
||||||
double Z = (Constants::e2m * n + h)*sphi;
|
|
||||||
|
|
||||||
result.setX(X);
|
|
||||||
result.setY(Y);
|
|
||||||
result.setZ(Z);
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CVectorGeo::zeros()
|
|
||||||
{
|
|
||||||
m_latitudeDegrees = 0;
|
|
||||||
m_longitudeDegrees = 0;
|
|
||||||
m_altitudeMeters = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CVectorGeo::print(std::ostream &stream)
|
|
||||||
{
|
|
||||||
stream << "v = " << std::endl;
|
|
||||||
stream << std::fixed;
|
|
||||||
stream << "[" << m_latitudeDegrees << "]" << std::endl;
|
|
||||||
stream << "[" << m_longitudeDegrees << "]" << std::endl;
|
|
||||||
stream << "[" << m_altitudeMeters << "]" << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
CVectorGeo &CVectorGeo::operator =(const CVectorGeo &rhs)
|
|
||||||
{
|
|
||||||
if (this != &rhs)
|
|
||||||
{
|
|
||||||
m_latitudeDegrees = rhs.latitudeDegrees();
|
|
||||||
m_longitudeDegrees = rhs.longitudeDegrees();
|
|
||||||
m_altitudeMeters = rhs.altitudeMeters();
|
|
||||||
}
|
|
||||||
return *this;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
@@ -1,58 +0,0 @@
|
|||||||
//! Copyright (C) 2013 Roland Winklmeier
|
|
||||||
//! 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/
|
|
||||||
|
|
||||||
#ifndef VECTOR_GEO_H
|
|
||||||
#define VECTOR_GEO_H
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
namespace BlackCore
|
|
||||||
{
|
|
||||||
|
|
||||||
class CEcef;
|
|
||||||
|
|
||||||
class CVectorGeo
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
CVectorGeo();
|
|
||||||
CVectorGeo(double latitudeDegrees, double longintudeDegrees, double altitudeMeters);
|
|
||||||
CVectorGeo(const CVectorGeo &other);
|
|
||||||
|
|
||||||
void setLatitudeDegrees(double latitudeDegrees)
|
|
||||||
{
|
|
||||||
m_latitudeDegrees = latitudeDegrees;
|
|
||||||
}
|
|
||||||
|
|
||||||
void setLongitudeDegrees(double longitudeDegrees)
|
|
||||||
{
|
|
||||||
m_longitudeDegrees = longitudeDegrees;
|
|
||||||
}
|
|
||||||
|
|
||||||
void setAltitude(double altitudeMeters)
|
|
||||||
{
|
|
||||||
m_altitudeMeters = altitudeMeters;
|
|
||||||
}
|
|
||||||
|
|
||||||
double latitudeDegrees() const { return m_latitudeDegrees; }
|
|
||||||
double longitudeDegrees() const { return m_longitudeDegrees; }
|
|
||||||
double altitudeMeters() const { return m_altitudeMeters; }
|
|
||||||
|
|
||||||
CEcef toCartesian();
|
|
||||||
|
|
||||||
void zeros();
|
|
||||||
|
|
||||||
void print(std::ostream &stream = std::cout);
|
|
||||||
|
|
||||||
CVectorGeo &operator=(const CVectorGeo &rhs);
|
|
||||||
|
|
||||||
private:
|
|
||||||
double m_latitudeDegrees;
|
|
||||||
double m_longitudeDegrees;
|
|
||||||
double m_altitudeMeters;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace BlackCore
|
|
||||||
|
|
||||||
#endif // VECTOR_GEO_H
|
|
||||||
Reference in New Issue
Block a user