codeboss
/
SimsWorld
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

添加机动命令执行代码

This commit is contained in:
codeboss 2025-06-21 15:56:59 +08:00
parent a0bb1feb8c
commit 304292d7f2
11 changed files with 305 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ComponentBasic/ComponentBasic.vcxproj
View File

@ -67,7 +67,7 @@
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<Link>
<AdditionalDependencies>MessageBasic.lib;SimsBasic.lib;%(AdditionalDependencies)</AdditionalDependencies>
<AdditionalDependencies>MessageBasic.lib;SimsBasic.lib;StandardGlobe.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'" Label="Configuration">

155
ComponentBasic/SurfaceMotionPlugin.cpp
View File

@ -1 +1,156 @@
#include "SurfaceMotionPlugin.h"
SurfaceMotionControlPlugin::SurfaceMotionControlPlugin()
{
}
void SurfaceMotionControlPlugin::recoveryFrom(const QJsonObject& obj)
{
}
void SurfaceMotionControlPlugin::saveTo(QJsonObject& obj) const
{
}
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
std::shared_ptr<const HorizontalArcMotion> in, QList<std::shared_ptr<RespondDefault>>& out)
{
auto exec_result = std::make_shared<RespondDefault>();
// 如果上一个机动命令存在，更新起始位置
if (_current_cmd) {
auto exec = std::make_shared<MotionDeduceRequest>();
exec->reset(in->targetEntity(), in->sourceEntity());
exec->_target_time = _current_cmd->startTimepoint();
map->execute(exec);
}
auto req = std::make_shared<Get3DBox>();
req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
auto rst = map->execute<Get3DBox, Box3DDesc>(req);
if (rst.size()) {
// 保存当前命令
this->_current_cmd = in;
// 更新基准坐标
auto item_result = rst.first();
this->_start_lla = item_result->_lla_pos;
this->_start_posture = item_result->_posture_d3;
exec_result->_success_mark = true;
}
else {
exec_result->_reason_text = "无法在指定实体（id=%1）获取%1消息，无法正常工作！";
}
out << exec_result;
}
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
std::shared_ptr<const StrightLineMotion> in, QList<std::shared_ptr<RespondDefault>>& out)
{
auto exec_result = std::make_shared<RespondDefault>();
// 如果上一个机动命令存在，更新起始位置
if (_current_cmd) {
auto exec = std::make_shared<MotionDeduceRequest>();
exec->reset(in->targetEntity(), in->sourceEntity());
exec->_target_time = _current_cmd->startTimepoint();
map->execute(exec);
}
// 提取起始机动状态
auto req = std::make_shared<Get3DBox>();
req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
auto rst = map->execute<Get3DBox, Box3DDesc>(req);
if (rst.size()) {
// 保存当前命令
this->_current_cmd = in;
// 更新基准坐标
auto item_result = rst.first();
this->_start_lla = item_result->_lla_pos;
this->_start_posture = item_result->_posture_d3;
exec_result->_success_mark = true;
}
else {
exec_result->_reason_text = "无法在指定实体（id=%1）获取%1类型消息，无法正常工作！";
}
out << exec_result;
}
#include <QMatrix4x4>
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
std::shared_ptr<const SyncRequest> in, QList<std::shared_ptr<RespondDefault>>& out)
{
auto reqs = std::make_shared<MotionDeduceRequest>();
reqs->reset(in->targetEntity(), in->sourceEntity());
reqs->_target_time = in->_time_current;
for (auto rst : map->execute(reqs)) {
auto conv = std::dynamic_pointer_cast<RespondDefault>(rst);
if (conv) out << conv;
}
}
#include <cmath>
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
std::shared_ptr<const MotionDeduceRequest> in, QList<std::shared_ptr<RespondDefault>>& out)
{
auto rst = std::make_shared<RespondDefault>();
rst->reset(in->targetEntity(), in->sourceEntity());
if (_current_cmd) {
// 机体坐标系推演
auto pos_d3 = _current_cmd->posAtTimespan(in->_target_time - _current_cmd->startTimepoint());
auto pos_p3 = _current_cmd->speedAtTimespan(in->_target_time - _current_cmd->startTimepoint());
QMatrix4x4 t;
t.rotate(-_start_posture._azimuth_deg, QVector3D(0, 0, 1));
// 本地坐标系结果
auto conv_pos = pos_d3 * t;
auto conv_psr = pos_p3 * t;
// 坐标转换，提取最终目标
auto azimuth_target_cos = QVector3D::dotProduct(conv_pos.normalized(), QVector3D(0, 1, 0));
auto azimuth_target = rad2d(std::acos(azimuth_target_cos));
if (QVector3D::crossProduct(conv_pos.normalized(), QVector3D(0, 1, 0)).z() < 0)
azimuth_target = 360 - azimuth_target;
LonLatAlt deduce_lla;
StandardGlobe::getTargetLLAWithDistance(_start_lla, conv_pos.length(), azimuth_target, deduce_lla);
// 朝向转换，提取方位角
auto azimuth_self_cos = QVector3D::dotProduct(conv_psr.normalized(), QVector3D(0, 1, 0));
auto azimuth_self = rad2d(std::acos(azimuth_self_cos));
if (QVector3D::crossProduct(conv_psr.normalized(), QVector3D(0, 1, 0)).z() < 0)
azimuth_self = 360 - azimuth_self;
auto setpos = std::make_shared<Set3DBoxLLAPos>();
setpos->_lla_pos = deduce_lla;
map->execute(setpos);
auto setpsr = std::make_shared<Set3DBoxPosture>();
setpsr->_posture_d3._azimuth_deg = azimuth_self;
map->execute(setpsr);
rst->_success_mark = true;
}
else {
rst->_reason_text = QString("实体%1未设置机动命令，无法响应“%2”类型消息。")
.arg(this->_bind_entity.lock()->entityID()).arg(in->topicString());
}
out << rst;
}
std::shared_ptr<WsComponent> SurfaceMotionControlPlugin::defaultNew() const
{
return std::make_shared<SurfaceMotionControlPlugin>();
}
void SurfaceMotionControlPlugin::bindEntity(std::weak_ptr<WsEntity> host)
{
this->_bind_entity = host;
}
QString SurfaceMotionControlPlugin::name() const
{
return NAME(SurfaceMotionControlPlugin);
}

29
ComponentBasic/SurfaceMotionPlugin.h
View File

@ -1,10 +1,37 @@
#pragma once
#include <motion_access.h>
#include "componentbasic.h"
/// <summary>
/// 水面平台机动组件
/// </summary>
class SurfaceMotionControlPlugin : public ProcList<
WsRespond<HorizontalArcMotion, RespondDefault>,
WsRespond<StrightLineMotion, RespondDefault>,
WsRespond<MotionDeduceRequest, RespondDefault>,
WsRespond<SyncRequest, RespondDefault>
>{
private:
std::weak_ptr<WsEntity> _bind_entity;
std::shared_ptr<const PlatformMotionCommand> _current_cmd = nullptr;
LonLatAlt _start_lla;
Posture _start_posture;
public:
SurfaceMotionControlPlugin();
// ͨ¹ý ProcList ¼Ì³Ð
void execute(std::shared_ptr<Immediate> map, std::shared_ptr<const HorizontalArcMotion> in, QList<std::shared_ptr<RespondDefault>>& out) override;
void execute(std::shared_ptr<Immediate> map, std::shared_ptr<const StrightLineMotion> in, QList<std::shared_ptr<RespondDefault>>& out) override;
void execute(std::shared_ptr<Immediate> map, std::shared_ptr<const MotionDeduceRequest> in, QList<std::shared_ptr<RespondDefault>>& out) override;
void execute(std::shared_ptr<Immediate> map, std::shared_ptr<const SyncRequest> in, QList<std::shared_ptr<RespondDefault>>& out) override;
std::shared_ptr<WsComponent> defaultNew() const override;
void bindEntity(std::weak_ptr<WsEntity> host) override;
QString name() const override;
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;
};

2
ComponentBasic/VisibleCube.h
View File

@ -16,7 +16,7 @@ class COMPONENTBASIC_EXPORT VisibleCubePlugin : public ProcList<
private:
std::weak_ptr<WsEntity> _bind_entity;
VolumeData _self_d3;
LonLatAltPos _self_lla;
LonLatAlt _self_lla;
Posture _self_posture;
public:

4
MessageBasic/messagebasic.h
View File

@ -105,7 +105,7 @@ struct VolumeData {
struct MESSAGEBASIC_EXPORT Box3DDesc : public AbstractMessage {
VolumeData _d3_data;
Posture _posture_d3;
LonLatAltPos _lla_pos;
LonLatAlt _lla_pos;
Box3DDesc();
@ -137,7 +137,7 @@ struct MESSAGEBASIC_EXPORT Set3DBoxPosture : public AbstractMessage
struct MESSAGEBASIC_EXPORT Set3DBoxLLAPos : public AbstractMessage
{
LonLatAltPos _lla_pos;
LonLatAlt _lla_pos;
Set3DBoxLLAPos();

51
MessageBasic/motion_access.cpp
View File

@ -39,12 +39,12 @@ PlatformMotionSequence::PlatformMotionSequence()
void PlatformMotionSequence::recoveryFrom(const QJsonObject& obj)
{
throw -1;
throw - 1;
}
void PlatformMotionSequence::saveTo(QJsonObject& obj) const
{
throw -1;
throw - 1;
}
#include <standardglobe.h>
@ -58,6 +58,11 @@ StrightLineMotion::StrightLineMotion()
: PlatformMotionCommand(NAME(StrightLineMotion)) {
}
double StrightLineMotion::startTimepoint() const
{
return _start_time;
}
double StrightLineMotion::timeExpend() const
{
auto sqrt_v = std::sqrt(_speed_motion * _speed_motion + 2 * _accerlate_rates * _length_total);
@ -65,7 +70,7 @@ double StrightLineMotion::timeExpend() const
}
#include <QVector3D>
QVector3D StrightLineMotion::posAtTime(double time) const
QVector3D StrightLineMotion::posAtTimespan(double time) const
{
auto length = _speed_motion * time +
0.5 * _accerlate_rates * time * time;
@ -73,7 +78,7 @@ QVector3D StrightLineMotion::posAtTime(double time) const
return QVector3D(0, length, 0);
}
QVector3D StrightLineMotion::speedAtTime(double time) const
QVector3D StrightLineMotion::speedAtTimespan(double time) const
{
auto vspeed = _speed_motion + _accerlate_rates * time;
return QVector3D(0, vspeed, 0);
@ -101,6 +106,11 @@ HorizontalArcMotion::HorizontalArcMotion()
: PlatformMotionCommand(NAME(HorizontalArcMotion)) {
}
double HorizontalArcMotion::startTimepoint() const
{
return _start_time;
}
double HorizontalArcMotion::timeExpend() const
{
auto radius = std::abs(_center_point.x());
@ -108,7 +118,7 @@ double HorizontalArcMotion::timeExpend() const
return arc_length / _speed_motion;
}
QVector3D HorizontalArcMotion::posAtTime(double time) const
QVector3D HorizontalArcMotion::posAtTimespan(double time) const
{
auto radius = std::abs(_center_point.x());
auto arc_length = _speed_motion * time;
@ -119,9 +129,9 @@ QVector3D HorizontalArcMotion::posAtTime(double time) const
return vec * radius + QVector3D(_center_point);
}
QVector3D HorizontalArcMotion::speedAtTime(double time) const
QVector3D HorizontalArcMotion::speedAtTimespan(double time) const
{
auto dist_vec = (posAtTime(time) - QVector3D(_center_point)).normalized();
auto dist_vec = (posAtTimespan(time) - QVector3D(_center_point)).normalized();
auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(0, 0, 1));
return speed_nv.normalized() * _speed_motion;
}
@ -160,6 +170,11 @@ VerticalArcMotion::VerticalArcMotion()
: PlatformMotionCommand(NAME(VerticalArcMotion)) {
}
double VerticalArcMotion::startTimepoint() const
{
return _start_time;
}
double VerticalArcMotion::timeExpend() const
{
auto radius = std::abs(_center_point.y());
@ -167,7 +182,7 @@ double VerticalArcMotion::timeExpend() const
return arc_length / _speed_motion;
}
QVector3D VerticalArcMotion::posAtTime(double time) const
QVector3D VerticalArcMotion::posAtTimespan(double time) const
{
auto radius = std::abs(_center_point.z());
auto arc_length = _speed_motion * time;
@ -178,9 +193,9 @@ QVector3D VerticalArcMotion::posAtTime(double time) const
return _center_point + vec_r * radius;
}
QVector3D VerticalArcMotion::speedAtTime(double time) const
QVector3D VerticalArcMotion::speedAtTimespan(double time) const
{
auto dist_vec = (posAtTime(time) - QVector3D(_center_point)).normalized();
auto dist_vec = (posAtTimespan(time) - QVector3D(_center_point)).normalized();
auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(1, 0, 0));
return speed_nv.normalized() * _speed_motion;
}
@ -210,3 +225,19 @@ void VerticalArcMotion::saveTo(QJsonObject& obj) const
DOUBLE_SAVE(_rotate_deg);
DOUBLE_SAVE(_speed_motion);
}
MotionDeduceRequest::MotionDeduceRequest()
: AbstractMessage(NAME(MotionDeduceRequest)) {
}
void MotionDeduceRequest::recoveryFrom(const QJsonObject& obj)
{
AbstractMessage::recoveryFrom(obj);
DOUBLE_PEAK(_target_time);
}
void MotionDeduceRequest::saveTo(QJsonObject& obj) const
{
AbstractMessage::saveTo(obj);
DOUBLE_SAVE(_target_time);
}

52
MessageBasic/motion_access.h
View File

@ -28,7 +28,11 @@ struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
/// </summary>
struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
PlatformMotionCommand(const QString &nm);
/// <summary>
/// 机动起始时间
/// </summary>
/// <returns></returns>
virtual double startTimepoint() const = 0;
/// <summary>
/// 消耗时间
/// </summary>
@ -39,13 +43,26 @@ struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
/// </summary>
/// <param name="time"></param>
/// <returns>机体坐标系</returns>
virtual QVector3D posAtTime(double time) const = 0;
virtual QVector3D posAtTimespan(double time) const = 0;
/// <summary>
/// 指定时间点的速度矢量
/// </summary>
/// <param name="time"></param>
/// <returns>机体坐标系</returns>
virtual QVector3D speedAtTime(double time) const = 0;
virtual QVector3D speedAtTimespan(double time) const = 0;
};
/// <summary>
/// 推演指定时间点机动状态
/// </summary>
struct MESSAGEBASIC_EXPORT MotionDeduceRequest : public AbstractMessage {
double _target_time = 0;
MotionDeduceRequest();
// Serializable
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;
};
/// <summary>
@ -69,12 +86,16 @@ struct MESSAGEBASIC_EXPORT PlatformMotionSequence : public AbstractMessage {
};
// 正交机动命令实现 =======================================================
// 机动命令实现 =======================================================
/// <summary>
/// 直线机动
/// </summary>
struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
/// <summary>
/// 起始时间
/// </summary>
double _start_time = 0;
/// <summary>
/// 机动速度，m/s
/// </summary>
@ -90,9 +111,10 @@ struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
StrightLineMotion();
double startTimepoint() const override;
double timeExpend() const override;
QVector3D posAtTime(double time) const override;
QVector3D speedAtTime(double time) const override;
QVector3D posAtTimespan(double time) const override;
QVector3D speedAtTimespan(double time) const override;
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;
@ -103,6 +125,10 @@ struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
/// 基于机体坐标系的水平标准圆弧机动
/// </summary>
struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
/// <summary>
/// 起始时间
/// </summary>
double _start_time = 0;
/// <summary>
/// 转弯圆心,<xpos, ypos, 0>
/// </summary>
@ -118,9 +144,10 @@ struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
HorizontalArcMotion();
double startTimepoint() const override;
double timeExpend() const override;
QVector3D posAtTime(double time) const override;
QVector3D speedAtTime(double time) const override;
QVector3D posAtTimespan(double time) const override;
QVector3D speedAtTimespan(double time) const override;
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;
@ -130,6 +157,10 @@ struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
/// 基于机体坐标系的垂直标准圆弧机动
/// </summary>
struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
/// <summary>
/// 起始时间
/// </summary>
double _start_time = 0;
/// <summary>
/// 转弯圆心,<0, ypos, zpos>
/// </summary>
@ -145,9 +176,10 @@ struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
VerticalArcMotion();
double startTimepoint() const override;
double timeExpend() const override;
QVector3D posAtTime(double time) const override;
QVector3D speedAtTime(double time) const override;
QVector3D posAtTimespan(double time) const override;
QVector3D speedAtTimespan(double time) const override;
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;

2
SimsBasic/internal_impl.h
View File

@ -171,6 +171,4 @@ public:
/// <param name="in"></param>
/// <returns></returns>
virtual QList<std::shared_ptr<WsMessage>> execute(std::shared_ptr<const WsMessage> in) override;
};

12
SimsBasic/simsbasic.h
View File

@ -87,6 +87,18 @@ public:
/// <param name="in"></param>
/// <returns></returns>
virtual QList<std::shared_ptr<WsMessage>> execute(const WsRespondSignatureType& resp_signature, std::shared_ptr<const WsMessage> in) = 0;
template<typename TypeIn, typename TypeOut>
QList<std::shared_ptr<TypeOut>> execute(std::shared_ptr<const WsMessage> in) {
auto sign = std::make_pair<QString, QString>(TypeIn().topicString(), TypeOut().topicString());
auto rest_list = this->execute(sign, in);
QList<std::shared_ptr<TypeOut>> typed_list;
for (auto it : rest_list) {
typed_list << std::static_pointer_cast<TypeOut>(it);
}
return typed_list;
}
/// <summary>
/// 实体内兼容调用立即执行
/// </summary>

23
StandardGlobe/standardglobe.cpp
View File

@ -3,15 +3,12 @@
#include <QVector3D>
double StandardGlobe::_radius_m = 65000000;
#define PI 3.14159265354
auto deg2a = [](double v) { return v / 180.0 * PI; };
auto rad2d = [](double v) { return v / PI * 180.0; };
StandardGlobe::StandardGlobe()
{
}
ECEFv3d StandardGlobe::llaToEcef(const LonLatAltPos& v)
ECEFv3d StandardGlobe::llaToEcef(const LonLatAlt& v)
{
// 柴麻鉦宣白伉鉦宣
auto axis_r_g = v._alt_m + _radius_m;
@ -27,7 +24,7 @@ ECEFv3d StandardGlobe::llaToEcef(const LonLatAltPos& v)
return ECEFv3d{ axis_x, axis_y, axis_z };
}
LonLatAltPos StandardGlobe::ecefToLLA(const ECEFv3d& v)
LonLatAlt 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);
@ -36,7 +33,7 @@ LonLatAltPos StandardGlobe::ecefToLLA(const ECEFv3d& v)
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);
LonLatAltPos ret;
LonLatAlt ret;
ret._lon_deg = rad2d(rad_lon);
ret._lat_deg = rad2d(rad_lat);
ret._alt_m = rx - _radius_m;
@ -44,7 +41,7 @@ LonLatAltPos StandardGlobe::ecefToLLA(const ECEFv3d& v)
return ret;
}
void StandardGlobe::getDistanceWithTargetLLA(const LonLatAltPos& base, const LonLatAltPos& target, double& dist, double& azi)
void StandardGlobe::getDistanceWithTargetLLA(const LonLatAlt& base, const LonLatAlt& target, double& dist, double& azi)
{
// ecef文楚
auto vec_base = llaToEcef(base).getVec3();
@ -75,7 +72,7 @@ void StandardGlobe::getDistanceWithTargetLLA(const LonLatAltPos& base, const Lon
}
#include <qquaternion.h>
void StandardGlobe::getTargetLLAWithDistance(const LonLatAltPos& base, double dist, double azi, LonLatAltPos& target)
void StandardGlobe::getTargetLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target)
{
auto ecef_bvec = llaToEcef(base).getVec3().normalized();
QQuaternion u0(-deg2a(azi), ecef_bvec);
@ -96,7 +93,7 @@ void StandardGlobe::getTargetLLAWithDistance(const LonLatAltPos& base, double di
target = ecefToLLA(sv);
}
PolarPos StandardGlobe::getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target)
PolarPos StandardGlobe::getPolarWithLLA(const LonLatAlt& base, const LonLatAlt& target)
{
// ecef文楚
auto vec_base = llaToEcef(base).getVec3();
@ -134,7 +131,7 @@ PolarPos StandardGlobe::getPolarWithLLA(const LonLatAltPos& base, const LonLatAl
return p;
}
LonLatAltPos StandardGlobe::getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target)
LonLatAlt StandardGlobe::getLLAWithPolar(const LonLatAlt& base, const PolarPos& target)
{
auto ecef_b = llaToEcef(base).getVec3();
auto u_azi = QQuaternion(-deg2a(target._azimuth_deg), ecef_b.normalized());
@ -164,17 +161,17 @@ ECEFv3d ECEFv3d::fromVec3(const QVector3D& p) {
return o;
}
LonLatAltPos::LonLatAltPos(const LonLatPos& plain_pos)
LonLatAlt::LonLatAlt(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)
LonLatAlt::LonLatAlt(double lon, double lat, double alt)
:_lon_deg(lon), _lat_deg(lat), _alt_m(alt) {
}
LonLatPos LonLatAltPos::toLonLatPos() const
LonLatPos LonLatAlt::toLonLatPos() const
{
return LonLatPos{ _lon_deg, _lat_deg };
}

22
StandardGlobe/standardglobe.h
View File

@ -2,6 +2,10 @@
#include "standardglobe_global.h"
#define PI 3.14159265354
#define deg2a(v) v / 180.0 * PI
#define rad2d(v) v / PI * 180.0
class QVector3D;
/// <summary>
@ -15,13 +19,13 @@ struct LonLatPos {
/// <summary>
/// 经纬高坐标系坐标
/// </summary>
struct STANDARDGLOBE_EXPORT LonLatAltPos {
struct STANDARDGLOBE_EXPORT LonLatAlt {
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);
LonLatAlt(double lon = 0, double lat = 0, double alt = 0);
LonLatAlt(const LonLatPos& plain_pos);
LonLatPos toLonLatPos() const;
};
@ -69,8 +73,8 @@ private:
public:
StandardGlobe();
static ECEFv3d llaToEcef(const LonLatAltPos& v);
static LonLatAltPos ecefToLLA(const ECEFv3d& v);
static ECEFv3d llaToEcef(const LonLatAlt& v);
static LonLatAlt ecefToLLA(const ECEFv3d& v);
/// <summary>
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角，warning：本计算忽视LLA的高度值
/// </summary>
@ -78,7 +82,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 LonLatAlt& base, const LonLatAlt& target, double& dist, double& azi);
/// <summary>
/// 根据两点之间的大地线长度和b->t初始方位角，计算目标LLA定位坐标
/// </summary>
@ -86,19 +90,19 @@ public:
/// <param name="dist"></param>
/// <param name="azi"></param>
/// <param name="target"></param>
static void getTargetLLAWithDistance(const LonLatAltPos& base, double dist, double azi, LonLatAltPos& target);
static void getTargetLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target);
/// <summary>
/// 通过LLA坐标，获取目标极坐标
/// </summary>
/// <param name="base"></param>
/// <param name="target"></param>
/// <returns></returns>
static PolarPos getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target);
static PolarPos getPolarWithLLA(const LonLatAlt& base, const LonLatAlt& target);
/// <summary>
/// 通过极坐标，获取目标LLA坐标
/// </summary>
/// <param name="base"></param>
/// <param name="target"></param>
/// <returns></returns>
static LonLatAltPos getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target);
static LonLatAlt getLLAWithPolar(const LonLatAlt& base, const PolarPos& target);
};