update:添加机体机动路径
This commit is contained in:
parent
300b31a578
commit
a0bb1feb8c
|
@ -61,6 +61,7 @@
|
||||||
<PropertyGroup Label="UserMacros" />
|
<PropertyGroup Label="UserMacros" />
|
||||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'">
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'">
|
||||||
<IncludePath>$(SolutionDir)SimsBasic;$(SolutionDir)StandardGlobe;$(IncludePath)</IncludePath>
|
<IncludePath>$(SolutionDir)SimsBasic;$(SolutionDir)StandardGlobe;$(IncludePath)</IncludePath>
|
||||||
|
<LibraryPath>$(SolutionDir)$(Platform)\$(Configuration);$(LibraryPath)</LibraryPath>
|
||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x64'">
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x64'">
|
||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
|
@ -68,6 +69,9 @@
|
||||||
<ClCompile>
|
<ClCompile>
|
||||||
<InlineFunctionExpansion>Default</InlineFunctionExpansion>
|
<InlineFunctionExpansion>Default</InlineFunctionExpansion>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<AdditionalDependencies>StandardGlobe.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
</ItemDefinitionGroup>
|
</ItemDefinitionGroup>
|
||||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'" Label="Configuration">
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'" Label="Configuration">
|
||||||
<ClCompile>
|
<ClCompile>
|
||||||
|
|
|
@ -10,6 +10,9 @@ void NavigateWithRoute::recoveryFrom(const QJsonObject& obj)
|
||||||
AbstractMessage::recoveryFrom(obj);
|
AbstractMessage::recoveryFrom(obj);
|
||||||
|
|
||||||
STRING_PEAK(_route_name);
|
STRING_PEAK(_route_name);
|
||||||
|
uint64_t _drive_mode;
|
||||||
|
UINT64_PEAK(_drive_mode);
|
||||||
|
this->_drive_mode = (NavigateDriveMode)_drive_mode;
|
||||||
}
|
}
|
||||||
|
|
||||||
void NavigateWithRoute::saveTo(QJsonObject& obj) const
|
void NavigateWithRoute::saveTo(QJsonObject& obj) const
|
||||||
|
@ -17,48 +20,12 @@ void NavigateWithRoute::saveTo(QJsonObject& obj) const
|
||||||
AbstractMessage::saveTo(obj);
|
AbstractMessage::saveTo(obj);
|
||||||
|
|
||||||
STRING_SAVE(_route_name);
|
STRING_SAVE(_route_name);
|
||||||
}
|
uint64_t _drive_mode = (uint64_t)this->_drive_mode;
|
||||||
|
UINT64_SAVE(_drive_mode);
|
||||||
PlatformMotionCommand::PlatformMotionCommand()
|
|
||||||
:AbstractMessage(NAME(PlatformMotionCommand))
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#include <QJsonArray>
|
#include <QJsonArray>
|
||||||
#include <QJsonObject>
|
#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()
|
MotionSequencePreviewGet::MotionSequencePreviewGet()
|
||||||
:AbstractMessage(NAME(MotionSequencePreviewGet)) {
|
:AbstractMessage(NAME(MotionSequencePreviewGet)) {
|
||||||
}
|
}
|
||||||
|
@ -72,44 +39,174 @@ PlatformMotionSequence::PlatformMotionSequence()
|
||||||
|
|
||||||
void PlatformMotionSequence::recoveryFrom(const QJsonObject& obj)
|
void PlatformMotionSequence::recoveryFrom(const QJsonObject& obj)
|
||||||
{
|
{
|
||||||
this->_cmd_sequence.clear();
|
throw -1;
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PlatformMotionSequence::saveTo(QJsonObject& obj) const
|
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);
|
AbstractMessage::saveTo(obj);
|
||||||
|
|
||||||
QJsonArray cmd_array;
|
DOUBLE_SAVE(_speed_motion);
|
||||||
for (auto info : _cmd_sequence) {
|
DOUBLE_SAVE(_accerlate_rates);
|
||||||
QJsonObject cmd_object;
|
DOUBLE_SAVE(_length_total);
|
||||||
info->saveTo(cmd_object);
|
|
||||||
cmd_array.append(cmd_object);
|
|
||||||
}
|
|
||||||
obj["cmd_array"] = cmd_array;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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
|
#pragma once
|
||||||
#include "messagebasic.h"
|
#include "messagebasic.h"
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 指定航路机动模式
|
||||||
|
/// </summary>
|
||||||
|
enum class NavigateDriveMode {
|
||||||
|
Cruise, // 巡航模式,长途奔袭模式
|
||||||
|
FullSpeed,// 全速模式,战斗机动
|
||||||
|
ReachStop, // 停驻模式
|
||||||
|
};
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 依照指定名称路径机动
|
/// 依照指定名称路径机动
|
||||||
/// </summary>
|
/// </summary>
|
||||||
struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
|
struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
|
||||||
QString _route_name;
|
QString _route_name; // 规划航线名称
|
||||||
|
NavigateDriveMode _drive_mode; // 驱动模式
|
||||||
|
|
||||||
NavigateWithRoute();
|
NavigateWithRoute();
|
||||||
|
|
||||||
|
@ -15,71 +24,28 @@ struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
|
||||||
};
|
};
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 机动类型
|
/// 平台机动命令,机体坐标系:主轴为Y,向右为X,向上为Z
|
||||||
/// </summary>
|
/// </summary>
|
||||||
enum MotionType {
|
struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
|
||||||
None, // 没有
|
PlatformMotionCommand(const QString &nm);
|
||||||
StraightLine,// 直线机动
|
|
||||||
StandardArc, // 标准圆弧
|
|
||||||
ParabolaLine,// 抛物线
|
|
||||||
Bezier2tArc, // 二次贝塞尔曲线
|
|
||||||
Bezier3tArc, // 三次贝塞尔曲线
|
|
||||||
};
|
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// 机动角度方向
|
|
||||||
/// </summary>
|
|
||||||
enum class MotionRotate {
|
|
||||||
Horizontal, Vertical
|
|
||||||
};
|
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// 正交机动命令抽象接口
|
|
||||||
/// </summary>
|
|
||||||
struct OrthogonalMotionCommand : public Serializable {
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 机动平面方向
|
/// 消耗时间
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <returns></returns>
|
/// <returns></returns>
|
||||||
virtual MotionRotate rotateType() const = 0;
|
virtual double timeExpend() const = 0;
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 机动类型
|
/// 指定时间点的相对位置
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <returns></returns>
|
/// <param name="time"></param>
|
||||||
virtual MotionType motionType() const = 0;
|
/// <returns>机体坐标系</returns>
|
||||||
|
virtual QVector3D posAtTime(double time) const = 0;
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 计算指定百分比位置
|
/// 指定时间点的速度矢量
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="percent"></param>
|
/// <param name="time"></param>
|
||||||
/// <returns></returns>
|
/// <returns>机体坐标系</returns>
|
||||||
virtual LonLatAltPos posAtPercent(double percent) = 0;
|
virtual QVector3D speedAtTime(double time) const = 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;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
@ -106,26 +72,83 @@ struct MESSAGEBASIC_EXPORT PlatformMotionSequence : public AbstractMessage {
|
||||||
// 正交机动命令实现 =======================================================
|
// 正交机动命令实现 =======================================================
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 水平直线机动
|
/// 直线机动
|
||||||
/// </summary>
|
/// </summary>
|
||||||
struct MESSAGEBASIC_EXPORT HorizontalLineMotionCmd : public OrthogonalMotionCommand {
|
struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
|
||||||
LonLatPos _lonlat_anchor;// 起始点
|
/// <summary>
|
||||||
double _azimuth_deg = 0; // 起始方位角
|
/// 机动速度,m/s
|
||||||
double _speed_value = 0;// 起始速度
|
/// </summary>
|
||||||
double _accerlate_rate = 0;// 加速度
|
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 recoveryFrom(const QJsonObject& obj) override;
|
||||||
void saveTo(QJsonObject& obj) const 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;
|
auto axis_r_g = v._alt_m + _radius_m;
|
||||||
|
@ -24,14 +24,14 @@ ECEFPos StandardGlobe::llaToEcef(const LonLatAltPos& v)
|
||||||
// ecef-y
|
// ecef-y
|
||||||
auto axis_y = axis_r_lat * sin(deg2a(v._lon_deg));
|
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 r2 = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos);
|
||||||
auto rad_lon = acos(v._x_pos / r2);
|
auto rad_lon = acos(v._x_pos / r2);
|
||||||
if(v._y_pos < 0)
|
if (v._y_pos < 0)
|
||||||
rad_lon *= -1;
|
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 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);
|
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);
|
QQuaternion u1(rad_between, vec_bv);
|
||||||
auto ecef_t = u1.rotatedVector(llaToEcef(base).getVec3());
|
auto ecef_t = u1.rotatedVector(llaToEcef(base).getVec3());
|
||||||
|
|
||||||
ECEFPos sv;
|
ECEFv3d sv;
|
||||||
sv._x_pos = ecef_t.x();
|
sv._x_pos = ecef_t.x();
|
||||||
sv._y_pos = ecef_t.y();
|
sv._y_pos = ecef_t.y();
|
||||||
sv._z_pos = ecef_t.z();
|
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 ecef_b = llaToEcef(base).getVec3();
|
||||||
auto u_azi = QQuaternion(-deg2a(target._azimuth_deg), ecef_b.normalized());
|
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 axis_pitch = QVector3D::crossProduct(vec_ts, ecef_b.normalized());
|
||||||
auto u_pit = QQuaternion(deg2a(target._pitch_deg), axis_pitch);
|
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 ts_appv = u_pit.rotatedVector(vec_ts).normalized() * r_shadow;
|
||||||
|
|
||||||
auto ecef_target = ecef_b + ts_appv;
|
auto ecef_target = ecef_b + ts_appv;
|
||||||
ECEFPos tt = ECEFPos::fromVec3(ecef_target);
|
ECEFv3d tt = ECEFv3d::fromVec3(ecef_target);
|
||||||
|
|
||||||
return ecefToLLA(tt);
|
return ecefToLLA(tt);
|
||||||
}
|
}
|
||||||
|
|
||||||
QVector3D ECEFPos::getVec3() const
|
QVector3D ECEFv3d::getVec3() const
|
||||||
{
|
{
|
||||||
return QVector3D(_x_pos, _y_pos, _z_pos);
|
return QVector3D(_x_pos, _y_pos, _z_pos);
|
||||||
}
|
}
|
||||||
ECEFPos ECEFPos::fromVec3(const QVector3D& p) {
|
ECEFv3d ECEFv3d::fromVec3(const QVector3D& p) {
|
||||||
ECEFPos o;
|
ECEFv3d o;
|
||||||
o._x_pos = p.x();
|
o._x_pos = p.x();
|
||||||
o._y_pos = p.y();
|
o._y_pos = p.y();
|
||||||
o._z_pos = p.z();
|
o._z_pos = p.z();
|
||||||
return o;
|
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>
|
||||||
/// 经纬高坐标系坐标
|
/// 经纬高坐标系坐标
|
||||||
/// </summary>
|
/// </summary>
|
||||||
struct LonLatAltPos {
|
struct STANDARDGLOBE_EXPORT LonLatAltPos {
|
||||||
double _lon_deg = 0;
|
double _lon_deg = 0;
|
||||||
double _lat_deg = 0;
|
double _lat_deg = 0;
|
||||||
double _alt_m = 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>
|
||||||
/// 地心坐标系
|
/// 地心坐标系
|
||||||
/// </summary>
|
/// </summary>
|
||||||
struct ECEFPos {
|
struct ECEFv3d {
|
||||||
double _x_pos = 0;// 指向0°经度圈与纬度0°交点
|
double _x_pos = 0;// 指向0°经度圈与纬度0°交点
|
||||||
double _y_pos = 0;
|
double _y_pos = 0;
|
||||||
double _z_pos = 0;
|
double _z_pos = 0;
|
||||||
QVector3D getVec3() const;
|
QVector3D getVec3() const;
|
||||||
static ECEFPos fromVec3(const QVector3D& p);
|
static ECEFv3d fromVec3(const QVector3D& p);
|
||||||
};
|
};
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 东天北坐标系
|
/// 东天北坐标系
|
||||||
/// </summary>
|
/// </summary>
|
||||||
struct ENUPos {
|
struct ENUv3d {
|
||||||
double _e_pos = 0;
|
double _e_pos = 0;
|
||||||
double _n_pos = 0;
|
double _n_pos = 0;
|
||||||
double _t_pos = 0;
|
double _t_pos = 0;
|
||||||
|
@ -64,8 +69,8 @@ private:
|
||||||
public:
|
public:
|
||||||
StandardGlobe();
|
StandardGlobe();
|
||||||
|
|
||||||
static ECEFPos llaToEcef(const LonLatAltPos &v);
|
static ECEFv3d llaToEcef(const LonLatAltPos& v);
|
||||||
static LonLatAltPos ecefToLLA(const ECEFPos &v);
|
static LonLatAltPos ecefToLLA(const ECEFv3d& v);
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角,warning:本计算忽视LLA的高度值
|
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角,warning:本计算忽视LLA的高度值
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
@ -73,7 +78,7 @@ public:
|
||||||
/// <param name="target"></param>
|
/// <param name="target"></param>
|
||||||
/// <param name="dist"></param>
|
/// <param name="dist"></param>
|
||||||
/// <param name="azi"></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>
|
/// <summary>
|
||||||
/// 根据两点之间的大地线长度和b->t初始方位角,计算目标LLA定位坐标
|
/// 根据两点之间的大地线长度和b->t初始方位角,计算目标LLA定位坐标
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
@ -88,12 +93,12 @@ public:
|
||||||
/// <param name="base"></param>
|
/// <param name="base"></param>
|
||||||
/// <param name="target"></param>
|
/// <param name="target"></param>
|
||||||
/// <returns></returns>
|
/// <returns></returns>
|
||||||
static PolarPos getPolarWithLLA(const LonLatAltPos &base, const LonLatAltPos &target);
|
static PolarPos getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target);
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 通过极坐标,获取目标LLA坐标
|
/// 通过极坐标,获取目标LLA坐标
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="base"></param>
|
/// <param name="base"></param>
|
||||||
/// <param name="target"></param>
|
/// <param name="target"></param>
|
||||||
/// <returns></returns>
|
/// <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