update：添加机体机动路径
This commit is contained in:
parent
300b31a578
commit
a0bb1feb8c
|
|
@ -61,6 +61,7 @@
|
|||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'">
|
||||
<IncludePath>$(SolutionDir)SimsBasic;$(SolutionDir)StandardGlobe;$(IncludePath)</IncludePath>
|
||||
<LibraryPath>$(SolutionDir)$(Platform)\$(Configuration);$(LibraryPath)</LibraryPath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x64'">
|
||||
</PropertyGroup>
|
||||
|
|
@ -68,6 +69,9 @@
|
|||
<ClCompile>
|
||||
<InlineFunctionExpansion>Default</InlineFunctionExpansion>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<AdditionalDependencies>StandardGlobe.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'" Label="Configuration">
|
||||
<ClCompile>
|
||||
|
|
|
|||
|
|
@ -10,6 +10,9 @@ void NavigateWithRoute::recoveryFrom(const QJsonObject& obj)
|
|||
AbstractMessage::recoveryFrom(obj);
|
||||
|
||||
STRING_PEAK(_route_name);
|
||||
uint64_t _drive_mode;
|
||||
UINT64_PEAK(_drive_mode);
|
||||
this->_drive_mode = (NavigateDriveMode)_drive_mode;
|
||||
}
|
||||
|
||||
void NavigateWithRoute::saveTo(QJsonObject& obj) const
|
||||
|
|
@ -17,48 +20,12 @@ void NavigateWithRoute::saveTo(QJsonObject& obj) const
|
|||
AbstractMessage::saveTo(obj);
|
||||
|
||||
STRING_SAVE(_route_name);
|
||||
}
|
||||
|
||||
PlatformMotionCommand::PlatformMotionCommand()
|
||||
:AbstractMessage(NAME(PlatformMotionCommand))
|
||||
{
|
||||
|
||||
uint64_t _drive_mode = (uint64_t)this->_drive_mode;
|
||||
UINT64_SAVE(_drive_mode);
|
||||
}
|
||||
|
||||
#include <QJsonArray>
|
||||
#include <QJsonObject>
|
||||
void PlatformMotionCommand::recoveryFrom(const QJsonObject& obj)
|
||||
{
|
||||
AbstractMessage::recoveryFrom(obj);
|
||||
DOUBLE_PEAK(_arrived_time);
|
||||
DOUBLE_PEAK(_start_time);
|
||||
|
||||
auto cmd_array = obj["array"].toArray();
|
||||
for (auto idx = 0; idx < cmd_array.size(); ++idx) {
|
||||
auto cmd_obj = cmd_array.at(idx).toObject();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void PlatformMotionCommand::saveTo(QJsonObject& obj) const
|
||||
{
|
||||
AbstractMessage::saveTo(obj);
|
||||
DOUBLE_SAVE(_arrived_time);
|
||||
DOUBLE_SAVE(_start_time);
|
||||
|
||||
QJsonArray cmd_array;
|
||||
for (auto k : this->_motion_set.keys()) {
|
||||
QJsonObject cmd;
|
||||
cmd["type"] = (int)k;
|
||||
QJsonObject objx;
|
||||
this->_motion_set[k]->saveTo(objx);
|
||||
cmd["content"] = objx;
|
||||
|
||||
cmd_array.append(cmd);
|
||||
}
|
||||
obj["array"] = cmd_array;
|
||||
}
|
||||
|
||||
MotionSequencePreviewGet::MotionSequencePreviewGet()
|
||||
:AbstractMessage(NAME(MotionSequencePreviewGet)) {
|
||||
}
|
||||
|
|
@ -72,44 +39,174 @@ PlatformMotionSequence::PlatformMotionSequence()
|
|||
|
||||
void PlatformMotionSequence::recoveryFrom(const QJsonObject& obj)
|
||||
{
|
||||
this->_cmd_sequence.clear();
|
||||
|
||||
AbstractMessage::recoveryFrom(obj);
|
||||
auto cmd_array = obj["cmd_array"].toArray();
|
||||
for (auto idx = 0; idx < cmd_array.size(); idx++) {
|
||||
auto cmd_object = cmd_array.at(idx).toObject();
|
||||
auto ptr = std::make_shared<PlatformMotionCommand>();
|
||||
ptr->recoveryFrom(cmd_object);
|
||||
this->_cmd_sequence.append(ptr);
|
||||
}
|
||||
throw -1;
|
||||
}
|
||||
|
||||
void PlatformMotionSequence::saveTo(QJsonObject& obj) const
|
||||
{
|
||||
throw -1;
|
||||
}
|
||||
|
||||
#include <standardglobe.h>
|
||||
|
||||
#define PI 3.14159265354
|
||||
auto deg2a = [](double v) { return v / 180.0 * PI; };
|
||||
auto rad2d = [](double v) { return v / PI * 180.0; };
|
||||
#include <QVector2D>
|
||||
#include <cmath>
|
||||
StrightLineMotion::StrightLineMotion()
|
||||
: PlatformMotionCommand(NAME(StrightLineMotion)) {
|
||||
}
|
||||
|
||||
double StrightLineMotion::timeExpend() const
|
||||
{
|
||||
auto sqrt_v = std::sqrt(_speed_motion * _speed_motion + 2 * _accerlate_rates * _length_total);
|
||||
return (sqrt_v - _speed_motion) / _accerlate_rates;
|
||||
}
|
||||
|
||||
#include <QVector3D>
|
||||
QVector3D StrightLineMotion::posAtTime(double time) const
|
||||
{
|
||||
auto length = _speed_motion * time +
|
||||
0.5 * _accerlate_rates * time * time;
|
||||
|
||||
return QVector3D(0, length, 0);
|
||||
}
|
||||
|
||||
QVector3D StrightLineMotion::speedAtTime(double time) const
|
||||
{
|
||||
auto vspeed = _speed_motion + _accerlate_rates * time;
|
||||
return QVector3D(0, vspeed, 0);
|
||||
}
|
||||
|
||||
void StrightLineMotion::recoveryFrom(const QJsonObject& obj)
|
||||
{
|
||||
AbstractMessage::recoveryFrom(obj);
|
||||
|
||||
DOUBLE_PEAK(_speed_motion);
|
||||
DOUBLE_PEAK(_accerlate_rates);
|
||||
DOUBLE_PEAK(_length_total);
|
||||
}
|
||||
|
||||
void StrightLineMotion::saveTo(QJsonObject& obj) const
|
||||
{
|
||||
AbstractMessage::saveTo(obj);
|
||||
|
||||
QJsonArray cmd_array;
|
||||
for (auto info : _cmd_sequence) {
|
||||
QJsonObject cmd_object;
|
||||
info->saveTo(cmd_object);
|
||||
cmd_array.append(cmd_object);
|
||||
}
|
||||
obj["cmd_array"] = cmd_array;
|
||||
DOUBLE_SAVE(_speed_motion);
|
||||
DOUBLE_SAVE(_accerlate_rates);
|
||||
DOUBLE_SAVE(_length_total);
|
||||
}
|
||||
|
||||
HorizontalLineMotionCmd::HorizontalLineMotionCmd(){}
|
||||
HorizontalArcMotion::HorizontalArcMotion()
|
||||
: PlatformMotionCommand(NAME(HorizontalArcMotion)) {
|
||||
}
|
||||
|
||||
MotionRotate HorizontalLineMotionCmd::rotateType() const
|
||||
double HorizontalArcMotion::timeExpend() const
|
||||
{
|
||||
MotionRotate::Horizontal;
|
||||
auto radius = std::abs(_center_point.x());
|
||||
auto arc_length = radius * deg2a(_rotate_deg);
|
||||
return arc_length / _speed_motion;
|
||||
}
|
||||
|
||||
MotionType HorizontalLineMotionCmd::motionType() const
|
||||
QVector3D HorizontalArcMotion::posAtTime(double time) const
|
||||
{
|
||||
MotionType::ParabolaLine;
|
||||
auto radius = std::abs(_center_point.x());
|
||||
auto arc_length = _speed_motion * time;
|
||||
auto _rotate_rads = arc_length / radius;
|
||||
|
||||
QVector3D vec(std::cos(_rotate_rads) * radius / _center_point.x(),
|
||||
std::sin(_rotate_rads), 0);
|
||||
return vec * radius + QVector3D(_center_point);
|
||||
}
|
||||
|
||||
LonLatAltPos HorizontalLineMotionCmd::posAtPercent(double percent)
|
||||
QVector3D HorizontalArcMotion::speedAtTime(double time) const
|
||||
{
|
||||
throw std::logic_error("The method or operation is not implemented.");
|
||||
auto dist_vec = (posAtTime(time) - QVector3D(_center_point)).normalized();
|
||||
auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(0, 0, 1));
|
||||
return speed_nv.normalized() * _speed_motion;
|
||||
}
|
||||
|
||||
void HorizontalArcMotion::recoveryFrom(const QJsonObject& obj)
|
||||
{
|
||||
AbstractMessage::recoveryFrom(obj);
|
||||
|
||||
double posx, posy, posz;
|
||||
DOUBLE_PEAK(posx);
|
||||
DOUBLE_PEAK(posy);
|
||||
DOUBLE_PEAK(posz);
|
||||
DOUBLE_PEAK(_rotate_deg);
|
||||
DOUBLE_PEAK(_speed_motion);
|
||||
|
||||
_center_point = QVector3D(posx, posy, posz);
|
||||
}
|
||||
|
||||
void HorizontalArcMotion::saveTo(QJsonObject& obj) const
|
||||
{
|
||||
AbstractMessage::saveTo(obj);
|
||||
|
||||
double posx = _center_point.x(), posy = _center_point.y(), posz = _center_point.z();
|
||||
DOUBLE_SAVE(posx);
|
||||
DOUBLE_SAVE(posy);
|
||||
DOUBLE_SAVE(posz);
|
||||
DOUBLE_SAVE(_rotate_deg);
|
||||
DOUBLE_SAVE(_speed_motion);
|
||||
}
|
||||
|
||||
PlatformMotionCommand::PlatformMotionCommand(const QString& nm)
|
||||
: AbstractMessage(nm) {
|
||||
}
|
||||
|
||||
VerticalArcMotion::VerticalArcMotion()
|
||||
: PlatformMotionCommand(NAME(VerticalArcMotion)) {
|
||||
}
|
||||
|
||||
double VerticalArcMotion::timeExpend() const
|
||||
{
|
||||
auto radius = std::abs(_center_point.y());
|
||||
auto arc_length = radius * deg2a(_rotate_deg);
|
||||
return arc_length / _speed_motion;
|
||||
}
|
||||
|
||||
QVector3D VerticalArcMotion::posAtTime(double time) const
|
||||
{
|
||||
auto radius = std::abs(_center_point.z());
|
||||
auto arc_length = _speed_motion * time;
|
||||
auto rotate_rads = arc_length / radius;
|
||||
|
||||
QVector3D vec_r(0, std::sin(rotate_rads),
|
||||
std::cos(rotate_rads) * radius / _center_point.z());
|
||||
return _center_point + vec_r * radius;
|
||||
}
|
||||
|
||||
QVector3D VerticalArcMotion::speedAtTime(double time) const
|
||||
{
|
||||
auto dist_vec = (posAtTime(time) - QVector3D(_center_point)).normalized();
|
||||
auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(1, 0, 0));
|
||||
return speed_nv.normalized() * _speed_motion;
|
||||
}
|
||||
|
||||
void VerticalArcMotion::recoveryFrom(const QJsonObject& obj)
|
||||
{
|
||||
AbstractMessage::recoveryFrom(obj);
|
||||
|
||||
double posx, posy, posz;
|
||||
DOUBLE_PEAK(posx);
|
||||
DOUBLE_PEAK(posy);
|
||||
DOUBLE_PEAK(posz);
|
||||
DOUBLE_PEAK(_rotate_deg);
|
||||
DOUBLE_PEAK(_speed_motion);
|
||||
|
||||
_center_point = QVector3D(posx, posy, posz);
|
||||
}
|
||||
|
||||
void VerticalArcMotion::saveTo(QJsonObject& obj) const
|
||||
{
|
||||
AbstractMessage::saveTo(obj);
|
||||
|
||||
double posx = _center_point.x(), posy = _center_point.y(), posz = _center_point.z();
|
||||
DOUBLE_SAVE(posx);
|
||||
DOUBLE_SAVE(posy);
|
||||
DOUBLE_SAVE(posz);
|
||||
DOUBLE_SAVE(_rotate_deg);
|
||||
DOUBLE_SAVE(_speed_motion);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,11 +1,20 @@
|
|||
#pragma once
|
||||
#include "messagebasic.h"
|
||||
|
||||
/// <summary>
|
||||
/// 指定航路机动模式
|
||||
/// </summary>
|
||||
enum class NavigateDriveMode {
|
||||
Cruise, // 巡航模式，长途奔袭模式
|
||||
FullSpeed,// 全速模式，战斗机动
|
||||
ReachStop, // 停驻模式
|
||||
};
|
||||
/// <summary>
|
||||
/// 依照指定名称路径机动
|
||||
/// </summary>
|
||||
struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
|
||||
QString _route_name;
|
||||
QString _route_name; // 规划航线名称
|
||||
NavigateDriveMode _drive_mode; // 驱动模式
|
||||
|
||||
NavigateWithRoute();
|
||||
|
||||
|
|
@ -15,71 +24,28 @@ struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
|
|||
};
|
||||
|
||||
/// <summary>
|
||||
/// 机动类型
|
||||
/// 平台机动命令，机体坐标系：主轴为Y，向右为X，向上为Z
|
||||
/// </summary>
|
||||
enum MotionType {
|
||||
None, // 没有
|
||||
StraightLine,// 直线机动
|
||||
StandardArc, // 标准圆弧
|
||||
ParabolaLine,// 抛物线
|
||||
Bezier2tArc, // 二次贝塞尔曲线
|
||||
Bezier3tArc, // 三次贝塞尔曲线
|
||||
};
|
||||
struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
|
||||
PlatformMotionCommand(const QString &nm);
|
||||
|
||||
/// <summary>
|
||||
/// 机动角度方向
|
||||
/// </summary>
|
||||
enum class MotionRotate {
|
||||
Horizontal, Vertical
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
/// 正交机动命令抽象接口
|
||||
/// </summary>
|
||||
struct OrthogonalMotionCommand : public Serializable {
|
||||
/// <summary>
|
||||
/// 机动平面方向
|
||||
/// 消耗时间
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
virtual MotionRotate rotateType() const = 0;
|
||||
virtual double timeExpend() const = 0;
|
||||
/// <summary>
|
||||
/// 机动类型
|
||||
/// 指定时间点的相对位置
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
virtual MotionType motionType() const = 0;
|
||||
/// <param name="time"></param>
|
||||
/// <returns>机体坐标系</returns>
|
||||
virtual QVector3D posAtTime(double time) const = 0;
|
||||
/// <summary>
|
||||
/// 计算指定百分比位置
|
||||
/// 指定时间点的速度矢量
|
||||
/// </summary>
|
||||
/// <param name="percent"></param>
|
||||
/// <returns></returns>
|
||||
virtual LonLatAltPos posAtPercent(double percent) = 0;
|
||||
/// <summary>
|
||||
/// 指定百分比位置的速度
|
||||
/// </summary>
|
||||
/// <param name="percent"></param>
|
||||
/// <returns></returns>
|
||||
virtual double speedAtPercent(double percent) = 0;
|
||||
/// <summary>
|
||||
/// 指定百分比位置速度方向
|
||||
/// </summary>
|
||||
/// <param name="percent"></param>
|
||||
/// <returns></returns>
|
||||
virtual QVector2D speedVectorAtPercent(double percent) = 0;
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
/// 平台机动命令
|
||||
/// </summary>
|
||||
struct PlatformMotionCommand : public AbstractMessage {
|
||||
QHash<MotionRotate, std::shared_ptr<OrthogonalMotionCommand>> _motion_set; //合成机动命令
|
||||
double _arrived_time = 0; // 抵达终点时间
|
||||
double _start_time = 0;
|
||||
|
||||
PlatformMotionCommand();
|
||||
|
||||
// Serializable
|
||||
void recoveryFrom(const QJsonObject& obj) override;
|
||||
void saveTo(QJsonObject& obj) const override;
|
||||
/// <param name="time"></param>
|
||||
/// <returns>机体坐标系</returns>
|
||||
virtual QVector3D speedAtTime(double time) const = 0;
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
|
|
@ -106,26 +72,83 @@ struct MESSAGEBASIC_EXPORT PlatformMotionSequence : public AbstractMessage {
|
|||
// 正交机动命令实现 =======================================================
|
||||
|
||||
/// <summary>
|
||||
/// 水平直线机动
|
||||
/// 直线机动
|
||||
/// </summary>
|
||||
struct MESSAGEBASIC_EXPORT HorizontalLineMotionCmd : public OrthogonalMotionCommand {
|
||||
LonLatPos _lonlat_anchor;// 起始点
|
||||
double _azimuth_deg = 0; // 起始方位角
|
||||
double _speed_value = 0;// 起始速度
|
||||
double _accerlate_rate = 0;// 加速度
|
||||
struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
|
||||
/// <summary>
|
||||
/// 机动速度，m/s
|
||||
/// </summary>
|
||||
double _speed_motion = 0;
|
||||
/// <summary>
|
||||
/// 加速度，m/s^2
|
||||
/// </summary>
|
||||
double _accerlate_rates = 0;
|
||||
/// <summary>
|
||||
/// 路径长度
|
||||
/// </summary>
|
||||
double _length_total = 0;
|
||||
|
||||
HorizontalLineMotionCmd();
|
||||
StrightLineMotion();
|
||||
|
||||
double timeExpend() const override;
|
||||
QVector3D posAtTime(double time) const override;
|
||||
QVector3D speedAtTime(double time) const override;
|
||||
|
||||
// Serializable
|
||||
void recoveryFrom(const QJsonObject& obj) override;
|
||||
void saveTo(QJsonObject& obj) const override;
|
||||
|
||||
MotionRotate rotateType() const override;
|
||||
MotionType motionType() const override;
|
||||
LonLatAltPos posAtPercent(double percent) override;
|
||||
|
||||
};
|
||||
|
||||
struct MESSAGEBASIC_EXPORT VerticalLineMotionCmd : public OrthogonalMotionCommand {
|
||||
#include <QVector3D>
|
||||
/// <summary>
|
||||
/// 基于机体坐标系的水平标准圆弧机动
|
||||
/// </summary>
|
||||
struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
|
||||
/// <summary>
|
||||
/// 转弯圆心,<xpos, ypos, 0>
|
||||
/// </summary>
|
||||
QVector3D _center_point;
|
||||
/// <summary>
|
||||
/// 弧线转角，°
|
||||
/// </summary>
|
||||
double _rotate_deg = 0;
|
||||
/// <summary>
|
||||
/// 机动速度，m/s
|
||||
/// </summary>
|
||||
double _speed_motion = 0;
|
||||
|
||||
HorizontalArcMotion();
|
||||
|
||||
double timeExpend() const override;
|
||||
QVector3D posAtTime(double time) const override;
|
||||
QVector3D speedAtTime(double time) const override;
|
||||
|
||||
void recoveryFrom(const QJsonObject& obj) override;
|
||||
void saveTo(QJsonObject& obj) const override;
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
/// 基于机体坐标系的垂直标准圆弧机动
|
||||
/// </summary>
|
||||
struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
|
||||
/// <summary>
|
||||
/// 转弯圆心,<0, ypos, zpos>
|
||||
/// </summary>
|
||||
QVector3D _center_point;
|
||||
/// <summary>
|
||||
/// 弧线转角，°
|
||||
/// </summary>
|
||||
double _rotate_deg = 0;
|
||||
/// <summary>
|
||||
/// 机动速度，m/s
|
||||
/// </summary>
|
||||
double _speed_motion = 0;
|
||||
|
||||
VerticalArcMotion();
|
||||
|
||||
double timeExpend() const override;
|
||||
QVector3D posAtTime(double time) const override;
|
||||
QVector3D speedAtTime(double time) const override;
|
||||
|
||||
void recoveryFrom(const QJsonObject& obj) override;
|
||||
void saveTo(QJsonObject& obj) const override;
|
||||
};
|
||||
|
|
@ -11,7 +11,7 @@ StandardGlobe::StandardGlobe()
|
|||
{
|
||||
}
|
||||
|
||||
ECEFPos StandardGlobe::llaToEcef(const LonLatAltPos& v)
|
||||
ECEFv3d StandardGlobe::llaToEcef(const LonLatAltPos& v)
|
||||
{
|
||||
// ¼ÆËã¾àÀëÇòÐľàÀë
|
||||
auto axis_r_g = v._alt_m + _radius_m;
|
||||
|
|
@ -24,14 +24,14 @@ ECEFPos StandardGlobe::llaToEcef(const LonLatAltPos& v)
|
|||
// ecef-y
|
||||
auto axis_y = axis_r_lat * sin(deg2a(v._lon_deg));
|
||||
|
||||
return ECEFPos{ axis_x, axis_y, axis_z };
|
||||
return ECEFv3d{ axis_x, axis_y, axis_z };
|
||||
}
|
||||
|
||||
LonLatAltPos StandardGlobe::ecefToLLA(const ECEFPos& v)
|
||||
LonLatAltPos StandardGlobe::ecefToLLA(const ECEFv3d& v)
|
||||
{
|
||||
auto r2 = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos);
|
||||
auto rad_lon = acos(v._x_pos / r2);
|
||||
if(v._y_pos < 0)
|
||||
if (v._y_pos < 0)
|
||||
rad_lon *= -1;
|
||||
auto rx = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos + v._z_pos * v._z_pos);
|
||||
auto rad_lat = asin(v._z_pos / rx);
|
||||
|
|
@ -89,7 +89,7 @@ void StandardGlobe::getTargetLLAWithDistance(const LonLatAltPos& base, double di
|
|||
QQuaternion u1(rad_between, vec_bv);
|
||||
auto ecef_t = u1.rotatedVector(llaToEcef(base).getVec3());
|
||||
|
||||
ECEFPos sv;
|
||||
ECEFv3d sv;
|
||||
sv._x_pos = ecef_t.x();
|
||||
sv._y_pos = ecef_t.y();
|
||||
sv._z_pos = ecef_t.z();
|
||||
|
|
@ -139,7 +139,7 @@ LonLatAltPos StandardGlobe::getLLAWithPolar(const LonLatAltPos& base, const Pola
|
|||
auto ecef_b = llaToEcef(base).getVec3();
|
||||
auto u_azi = QQuaternion(-deg2a(target._azimuth_deg), ecef_b.normalized());
|
||||
|
||||
auto vec_ts = u_azi.rotatedVector(QVector3D(0,0,1)).normalized();
|
||||
auto vec_ts = u_azi.rotatedVector(QVector3D(0, 0, 1)).normalized();
|
||||
auto axis_pitch = QVector3D::crossProduct(vec_ts, ecef_b.normalized());
|
||||
auto u_pit = QQuaternion(deg2a(target._pitch_deg), axis_pitch);
|
||||
|
||||
|
|
@ -147,19 +147,34 @@ LonLatAltPos StandardGlobe::getLLAWithPolar(const LonLatAltPos& base, const Pola
|
|||
auto ts_appv = u_pit.rotatedVector(vec_ts).normalized() * r_shadow;
|
||||
|
||||
auto ecef_target = ecef_b + ts_appv;
|
||||
ECEFPos tt = ECEFPos::fromVec3(ecef_target);
|
||||
ECEFv3d tt = ECEFv3d::fromVec3(ecef_target);
|
||||
|
||||
return ecefToLLA(tt);
|
||||
}
|
||||
|
||||
QVector3D ECEFPos::getVec3() const
|
||||
QVector3D ECEFv3d::getVec3() const
|
||||
{
|
||||
return QVector3D(_x_pos, _y_pos, _z_pos);
|
||||
}
|
||||
ECEFPos ECEFPos::fromVec3(const QVector3D& p) {
|
||||
ECEFPos o;
|
||||
ECEFv3d ECEFv3d::fromVec3(const QVector3D& p) {
|
||||
ECEFv3d o;
|
||||
o._x_pos = p.x();
|
||||
o._y_pos = p.y();
|
||||
o._z_pos = p.z();
|
||||
return o;
|
||||
}
|
||||
|
||||
LonLatAltPos::LonLatAltPos(const LonLatPos& plain_pos)
|
||||
: _lon_deg(plain_pos._lon_deg),
|
||||
_lat_deg(plain_pos._lat_deg),
|
||||
_alt_m(0) {
|
||||
}
|
||||
|
||||
LonLatAltPos::LonLatAltPos(double lon, double lat, double alt)
|
||||
:_lon_deg(lon), _lat_deg(lat), _alt_m(alt) {
|
||||
}
|
||||
|
||||
LonLatPos LonLatAltPos::toLonLatPos() const
|
||||
{
|
||||
return LonLatPos{ _lon_deg, _lat_deg };
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,27 +15,32 @@ struct LonLatPos {
|
|||
/// <summary>
|
||||
/// 经纬高坐标系坐标
|
||||
/// </summary>
|
||||
struct LonLatAltPos {
|
||||
struct STANDARDGLOBE_EXPORT LonLatAltPos {
|
||||
double _lon_deg = 0;
|
||||
double _lat_deg = 0;
|
||||
double _alt_m = 0;
|
||||
|
||||
LonLatAltPos(double lon = 0, double lat = 0, double alt = 0);
|
||||
LonLatAltPos(const LonLatPos& plain_pos);
|
||||
|
||||
LonLatPos toLonLatPos() const;
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
/// 地心坐标系
|
||||
/// </summary>
|
||||
struct ECEFPos {
|
||||
struct ECEFv3d {
|
||||
double _x_pos = 0;// 指向0°经度圈与纬度0°交点
|
||||
double _y_pos = 0;
|
||||
double _z_pos = 0;
|
||||
QVector3D getVec3() const;
|
||||
static ECEFPos fromVec3(const QVector3D& p);
|
||||
static ECEFv3d fromVec3(const QVector3D& p);
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
/// 东天北坐标系
|
||||
/// </summary>
|
||||
struct ENUPos {
|
||||
struct ENUv3d {
|
||||
double _e_pos = 0;
|
||||
double _n_pos = 0;
|
||||
double _t_pos = 0;
|
||||
|
|
@ -64,8 +69,8 @@ private:
|
|||
public:
|
||||
StandardGlobe();
|
||||
|
||||
static ECEFPos llaToEcef(const LonLatAltPos &v);
|
||||
static LonLatAltPos ecefToLLA(const ECEFPos &v);
|
||||
static ECEFv3d llaToEcef(const LonLatAltPos& v);
|
||||
static LonLatAltPos ecefToLLA(const ECEFv3d& v);
|
||||
/// <summary>
|
||||
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角，warning：本计算忽视LLA的高度值
|
||||
/// </summary>
|
||||
|
|
@ -73,7 +78,7 @@ public:
|
|||
/// <param name="target"></param>
|
||||
/// <param name="dist"></param>
|
||||
/// <param name="azi"></param>
|
||||
static void getDistanceWithTargetLLA(const LonLatAltPos &base, const LonLatAltPos &target, double &dist, double &azi);
|
||||
static void getDistanceWithTargetLLA(const LonLatAltPos& base, const LonLatAltPos& target, double& dist, double& azi);
|
||||
/// <summary>
|
||||
/// 根据两点之间的大地线长度和b->t初始方位角，计算目标LLA定位坐标
|
||||
/// </summary>
|
||||
|
|
@ -88,12 +93,12 @@ public:
|
|||
/// <param name="base"></param>
|
||||
/// <param name="target"></param>
|
||||
/// <returns></returns>
|
||||
static PolarPos getPolarWithLLA(const LonLatAltPos &base, const LonLatAltPos &target);
|
||||
static PolarPos getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target);
|
||||
/// <summary>
|
||||
/// 通过极坐标，获取目标LLA坐标
|
||||
/// </summary>
|
||||
/// <param name="base"></param>
|
||||
/// <param name="target"></param>
|
||||
/// <returns></returns>
|
||||
static LonLatAltPos getLLAWithPolar(const LonLatAltPos &base, const PolarPos &target);
|
||||
static LonLatAltPos getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target);
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue