添加机动命令执行代码

ComponentBasic/ComponentBasic.vcxproj

@@ -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">

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<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);
+}

ComponentBasic/SurfaceMotionPlugin.h

@@ -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;
+
 };
 

ComponentBasic/VisibleCube.h

@@ -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:

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();
 

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 <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);
+}

MessageBasic/motion_access.h

@@ -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 posAtTimespan(double time) const override;
-	QVector3D speedAtTime(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 posAtTimespan(double time) const override;
-	QVector3D speedAtTime(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 posAtTimespan(double time) const override;
-	QVector3D speedAtTime(double time) const override;
+	QVector3D speedAtTimespan(double time) const override;
 
 	void recoveryFrom(const QJsonObject& obj) override;
 	void saveTo(QJsonObject& obj) const override;

SimsBasic/internal_impl.h

@@ -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;
-
-
 };

SimsBasic/simsbasic.h

@@ -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>

StandardGlobe/standardglobe.cpp

@@ -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 };
 }

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;
 
 /// <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);
+	LonLatAlt(double lon = 0, double lat = 0, double alt = 0);
-	LonLatAltPos(const LonLatPos& plain_pos);
+	LonLatAlt(const LonLatPos& plain_pos);
 
 	LonLatPos toLonLatPos() const;
 };
@@ -69,8 +73,8 @@ private:
 public:
 	StandardGlobe();
 
-	static ECEFv3d llaToEcef(const LonLatAltPos& v);
+	static ECEFv3d llaToEcef(const LonLatAlt& v);
-	static LonLatAltPos ecefToLLA(const ECEFv3d& 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);
 };