From 304292d7f2e8f3fc371a6d6106b62f05573cd096 Mon Sep 17 00:00:00 2001
From: codeboss <2422523675@qq.com>
Date: Sat, 21 Jun 2025 15:56:59 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=9C=BA=E5=8A=A8=E5=91=BD?=
=?UTF-8?q?=E4=BB=A4=E6=89=A7=E8=A1=8C=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
ComponentBasic/ComponentBasic.vcxproj | 2 +-
ComponentBasic/SurfaceMotionPlugin.cpp | 155 +++++++++++++++++++++++++
ComponentBasic/SurfaceMotionPlugin.h | 29 ++++-
ComponentBasic/VisibleCube.h | 2 +-
MessageBasic/messagebasic.h | 4 +-
MessageBasic/motion_access.cpp | 51 ++++++--
MessageBasic/motion_access.h | 52 +++++++--
SimsBasic/internal_impl.h | 2 -
SimsBasic/simsbasic.h | 12 ++
StandardGlobe/standardglobe.cpp | 23 ++--
StandardGlobe/standardglobe.h | 22 ++--
11 files changed, 305 insertions(+), 49 deletions(-)
diff --git a/ComponentBasic/ComponentBasic.vcxproj b/ComponentBasic/ComponentBasic.vcxproj
index 608216c..30c26f6 100644
--- a/ComponentBasic/ComponentBasic.vcxproj
+++ b/ComponentBasic/ComponentBasic.vcxproj
@@ -67,7 +67,7 @@
- MessageBasic.lib;SimsBasic.lib;%(AdditionalDependencies)
+ MessageBasic.lib;SimsBasic.lib;StandardGlobe.lib;%(AdditionalDependencies)
diff --git a/ComponentBasic/SurfaceMotionPlugin.cpp b/ComponentBasic/SurfaceMotionPlugin.cpp
index 35249ac..89abaa5 100644
--- a/ComponentBasic/SurfaceMotionPlugin.cpp
+++ b/ComponentBasic/SurfaceMotionPlugin.cpp
@@ -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 map,
+ std::shared_ptr in, QList>& out)
+{
+ auto exec_result = std::make_shared();
+ // 如果上一个机动命令存在，更新起始位置
+ if (_current_cmd) {
+ auto exec = std::make_shared();
+ exec->reset(in->targetEntity(), in->sourceEntity());
+ exec->_target_time = _current_cmd->startTimepoint();
+ map->execute(exec);
+ }
+
+ auto req = std::make_shared();
+ req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
+ auto rst = map->execute(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 map,
+ std::shared_ptr in, QList>& out)
+{
+ auto exec_result = std::make_shared();
+ // 如果上一个机动命令存在，更新起始位置
+ if (_current_cmd) {
+ auto exec = std::make_shared();
+ exec->reset(in->targetEntity(), in->sourceEntity());
+ exec->_target_time = _current_cmd->startTimepoint();
+ map->execute(exec);
+ }
+
+ // 提取起始机动状态
+ auto req = std::make_shared();
+ req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
+ auto rst = map->execute(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
+void SurfaceMotionControlPlugin::execute(std::shared_ptr map,
+ std::shared_ptr in, QList>& out)
+{
+ auto reqs = std::make_shared();
+ reqs->reset(in->targetEntity(), in->sourceEntity());
+ reqs->_target_time = in->_time_current;
+
+ for (auto rst : map->execute(reqs)) {
+ auto conv = std::dynamic_pointer_cast(rst);
+ if (conv) out << conv;
+ }
+}
+
+#include
+void SurfaceMotionControlPlugin::execute(std::shared_ptr map,
+ std::shared_ptr in, QList>& out)
+{
+ auto rst = std::make_shared();
+ 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();
+ setpos->_lla_pos = deduce_lla;
+ map->execute(setpos);
+
+ auto setpsr = std::make_shared();
+ 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 SurfaceMotionControlPlugin::defaultNew() const
+{
+ return std::make_shared();
+}
+
+void SurfaceMotionControlPlugin::bindEntity(std::weak_ptr host)
+{
+ this->_bind_entity = host;
+}
+
+QString SurfaceMotionControlPlugin::name() const
+{
+ return NAME(SurfaceMotionControlPlugin);
+}
diff --git a/ComponentBasic/SurfaceMotionPlugin.h b/ComponentBasic/SurfaceMotionPlugin.h
index 15f8e7d..a0fda0f 100644
--- a/ComponentBasic/SurfaceMotionPlugin.h
+++ b/ComponentBasic/SurfaceMotionPlugin.h
@@ -1,10 +1,37 @@
#pragma once
+#include
+#include "componentbasic.h"
///
/// 水面平台机动组件
///
class SurfaceMotionControlPlugin : public ProcList<
-
+ WsRespond,
+ WsRespond,
+ WsRespond,
+ WsRespond
>{
+private:
+ std::weak_ptr _bind_entity;
+ std::shared_ptr _current_cmd = nullptr;
+ LonLatAlt _start_lla;
+ Posture _start_posture;
+
+public:
+ SurfaceMotionControlPlugin();
+
+ // 通过 ProcList 继承
+ void execute(std::shared_ptr map, std::shared_ptr in, QList>& out) override;
+ void execute(std::shared_ptr map, std::shared_ptr in, QList>& out) override;
+ void execute(std::shared_ptr map, std::shared_ptr in, QList>& out) override;
+ void execute(std::shared_ptr map, std::shared_ptr in, QList>& out) override;
+
+ std::shared_ptr defaultNew() const override;
+ void bindEntity(std::weak_ptr host) override;
+ QString name() const override;
+
+ void recoveryFrom(const QJsonObject& obj) override;
+ void saveTo(QJsonObject& obj) const override;
+
};
diff --git a/ComponentBasic/VisibleCube.h b/ComponentBasic/VisibleCube.h
index 61e1838..57d583d 100644
--- a/ComponentBasic/VisibleCube.h
+++ b/ComponentBasic/VisibleCube.h
@@ -16,7 +16,7 @@ class COMPONENTBASIC_EXPORT VisibleCubePlugin : public ProcList<
private:
std::weak_ptr _bind_entity;
VolumeData _self_d3;
- LonLatAltPos _self_lla;
+ LonLatAlt _self_lla;
Posture _self_posture;
public:
diff --git a/MessageBasic/messagebasic.h b/MessageBasic/messagebasic.h
index 26bcd05..9488ded 100644
--- a/MessageBasic/messagebasic.h
+++ b/MessageBasic/messagebasic.h
@@ -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();
diff --git a/MessageBasic/motion_access.cpp b/MessageBasic/motion_access.cpp
index 71b7987..8b7a645 100644
--- a/MessageBasic/motion_access.cpp
+++ b/MessageBasic/motion_access.cpp
@@ -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
@@ -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 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);
+}
diff --git a/MessageBasic/motion_access.h b/MessageBasic/motion_access.h
index 7903595..e4858b5 100644
--- a/MessageBasic/motion_access.h
+++ b/MessageBasic/motion_access.h
@@ -28,7 +28,11 @@ struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
///
struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
PlatformMotionCommand(const QString &nm);
-
+ ///
+ /// 机动起始时间
+ ///
+ ///
+ virtual double startTimepoint() const = 0;
///
/// 消耗时间
///
@@ -39,13 +43,26 @@ struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
///
///
/// 机体坐标系
- virtual QVector3D posAtTime(double time) const = 0;
+ virtual QVector3D posAtTimespan(double time) const = 0;
///
/// 指定时间点的速度矢量
///
///
/// 机体坐标系
- virtual QVector3D speedAtTime(double time) const = 0;
+ virtual QVector3D speedAtTimespan(double time) const = 0;
+};
+
+///
+/// 推演指定时间点机动状态
+///
+struct MESSAGEBASIC_EXPORT MotionDeduceRequest : public AbstractMessage {
+ double _target_time = 0;
+
+ MotionDeduceRequest();
+
+ // Serializable
+ void recoveryFrom(const QJsonObject& obj) override;
+ void saveTo(QJsonObject& obj) const override;
};
///
@@ -69,12 +86,16 @@ struct MESSAGEBASIC_EXPORT PlatformMotionSequence : public AbstractMessage {
};
-// 正交机动命令实现 =======================================================
+// 机动命令实现 =======================================================
///
/// 直线机动
///
struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
+ ///
+ /// 起始时间
+ ///
+ double _start_time = 0;
///
/// 机动速度，m/s
///
@@ -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 {
/// 基于机体坐标系的水平标准圆弧机动
///
struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
+ ///
+ /// 起始时间
+ ///
+ double _start_time = 0;
///
/// 转弯圆心,
///
@@ -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 {
/// 基于机体坐标系的垂直标准圆弧机动
///
struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
+ ///
+ /// 起始时间
+ ///
+ double _start_time = 0;
///
/// 转弯圆心,<0, ypos, zpos>
///
@@ -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;
diff --git a/SimsBasic/internal_impl.h b/SimsBasic/internal_impl.h
index 58b7f35..740e88a 100644
--- a/SimsBasic/internal_impl.h
+++ b/SimsBasic/internal_impl.h
@@ -171,6 +171,4 @@ public:
///
///
virtual QList> execute(std::shared_ptr in) override;
-
-
};
\ No newline at end of file
diff --git a/SimsBasic/simsbasic.h b/SimsBasic/simsbasic.h
index 3e973a6..f85ffc0 100644
--- a/SimsBasic/simsbasic.h
+++ b/SimsBasic/simsbasic.h
@@ -87,6 +87,18 @@ public:
///
///
virtual QList> execute(const WsRespondSignatureType& resp_signature, std::shared_ptr in) = 0;
+
+ template
+ QList> execute(std::shared_ptr in) {
+ auto sign = std::make_pair(TypeIn().topicString(), TypeOut().topicString());
+ auto rest_list = this->execute(sign, in);
+
+ QList> typed_list;
+ for (auto it : rest_list) {
+ typed_list << std::static_pointer_cast(it);
+ }
+ return typed_list;
+ }
///
/// 实体内兼容调用立即执行
///
diff --git a/StandardGlobe/standardglobe.cpp b/StandardGlobe/standardglobe.cpp
index df6e5ee..62ae04e 100644
--- a/StandardGlobe/standardglobe.cpp
+++ b/StandardGlobe/standardglobe.cpp
@@ -3,15 +3,12 @@
#include
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
-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 };
}
diff --git a/StandardGlobe/standardglobe.h b/StandardGlobe/standardglobe.h
index 1fcb411..3603301 100644
--- a/StandardGlobe/standardglobe.h
+++ b/StandardGlobe/standardglobe.h
@@ -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;
///
@@ -15,13 +19,13 @@ struct LonLatPos {
///
/// 经纬高坐标系坐标
///
-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);
///
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角，warning：本计算忽视LLA的高度值
///
@@ -78,7 +82,7 @@ public:
///
///
///
- 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);
///
/// 根据两点之间的大地线长度和b->t初始方位角，计算目标LLA定位坐标
///
@@ -86,19 +90,19 @@ public:
///
///
///
- static void getTargetLLAWithDistance(const LonLatAltPos& base, double dist, double azi, LonLatAltPos& target);
+ static void getTargetLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target);
///
/// 通过LLA坐标，获取目标极坐标
///
///
///
///
- static PolarPos getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target);
+ static PolarPos getPolarWithLLA(const LonLatAlt& base, const LonLatAlt& target);
///
/// 通过极坐标，获取目标LLA坐标
///
///
///
///
- static LonLatAltPos getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target);
+ static LonLatAlt getLLAWithPolar(const LonLatAlt& base, const PolarPos& target);
};