From 86cbc5a9d632171f0eadddcf925adec38701cdda Mon Sep 17 00:00:00 2001 From: codeboss <2422523675@qq.com> Date: Sun, 22 Jun 2025 09:58:16 +0800 Subject: [PATCH] =?UTF-8?q?=E9=AA=8C=E8=AF=81=E5=92=8C=E4=BF=AE=E5=A4=8D?= =?UTF-8?q?=E6=9C=BA=E5=8A=A8=E7=BB=84=E4=BB=B6StrightLineCmd?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ComponentBasic/SurfaceMotionPlugin.cpp | 8 +- ComponentBasic/SurfaceMotionPlugin.h | 2 +- ComponentBasic/VisibleCube.h | 2 +- MessageBasic/motion_access.cpp | 3 + SimsWorld.sln | 13 +++ StandardGlobe/standardglobe.cpp | 37 ++++---- StandardGlobe/standardglobe.h | 6 +- TestProject/TestProject.vcxproj | 110 ++++++++++++++++++++++++ TestProject/TestProject.vcxproj.filters | 31 +++++++ TestProject/TestProject.vcxproj.user | 12 +++ TestProject/main.cpp | 47 ++++++++++ 11 files changed, 248 insertions(+), 23 deletions(-) create mode 100644 TestProject/TestProject.vcxproj create mode 100644 TestProject/TestProject.vcxproj.filters create mode 100644 TestProject/TestProject.vcxproj.user create mode 100644 TestProject/main.cpp diff --git a/ComponentBasic/SurfaceMotionPlugin.cpp b/ComponentBasic/SurfaceMotionPlugin.cpp index 89abaa5..efb9d6c 100644 --- a/ComponentBasic/SurfaceMotionPlugin.cpp +++ b/ComponentBasic/SurfaceMotionPlugin.cpp @@ -39,7 +39,8 @@ void SurfaceMotionControlPlugin::execute(std::shared_ptr map, exec_result->_success_mark = true; } else { - exec_result->_reason_text = "ÎÞ·¨ÔÚÖ¸¶¨ÊµÌ壨id=%1£©»ñÈ¡%1ÏûÏ¢£¬ÎÞ·¨Õý³£¹¤×÷£¡"; + exec_result->_reason_text = QString(u8"ÎÞ·¨ÔÚÖ¸¶¨ÊµÌ壨id=%1£©»ñÈ¡%2ÏûÏ¢£¬ÎÞ·¨Õý³£¹¤×÷£¡") + .arg(this->_bind_entity.lock()->entityID()).arg(Box3DDesc().topicString()); } out << exec_result; } @@ -71,7 +72,8 @@ void SurfaceMotionControlPlugin::execute(std::shared_ptr map, exec_result->_success_mark = true; } else { - exec_result->_reason_text = "ÎÞ·¨ÔÚÖ¸¶¨ÊµÌ壨id=%1£©»ñÈ¡%1ÀàÐÍÏûÏ¢£¬ÎÞ·¨Õý³£¹¤×÷£¡"; + exec_result->_reason_text = QString(u8"ÎÞ·¨ÔÚÖ¸¶¨ÊµÌ壨id=%1£©»ñÈ¡%2ÏûÏ¢£¬ÎÞ·¨Õý³£¹¤×÷£¡") + .arg(this->_bind_entity.lock()->entityID()).arg(Box3DDesc().topicString()); } out << exec_result; } @@ -114,7 +116,7 @@ void SurfaceMotionControlPlugin::execute(std::shared_ptr map, 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); + StandardGlobe::getLLAWithDistance(_start_lla, conv_pos.length(), azimuth_target, deduce_lla); // ³¯Ïòת»»£¬ÌáÈ¡·½Î»½Ç auto azimuth_self_cos = QVector3D::dotProduct(conv_psr.normalized(), QVector3D(0, 1, 0)); diff --git a/ComponentBasic/SurfaceMotionPlugin.h b/ComponentBasic/SurfaceMotionPlugin.h index a0fda0f..c53c1c0 100644 --- a/ComponentBasic/SurfaceMotionPlugin.h +++ b/ComponentBasic/SurfaceMotionPlugin.h @@ -5,7 +5,7 @@ /// /// Ë®Ãæƽ̨»ú¶¯×é¼þ /// -class SurfaceMotionControlPlugin : public ProcList< +class COMPONENTBASIC_EXPORT SurfaceMotionControlPlugin : public ProcList< WsRespond, WsRespond, WsRespond, diff --git a/ComponentBasic/VisibleCube.h b/ComponentBasic/VisibleCube.h index 57d583d..b267c35 100644 --- a/ComponentBasic/VisibleCube.h +++ b/ComponentBasic/VisibleCube.h @@ -13,7 +13,7 @@ class COMPONENTBASIC_EXPORT VisibleCubePlugin : public ProcList< WsRespond > { -private: +public: std::weak_ptr _bind_entity; VolumeData _self_d3; LonLatAlt _self_lla; diff --git a/MessageBasic/motion_access.cpp b/MessageBasic/motion_access.cpp index 8b7a645..ac7a3dd 100644 --- a/MessageBasic/motion_access.cpp +++ b/MessageBasic/motion_access.cpp @@ -65,6 +65,9 @@ double StrightLineMotion::startTimepoint() const double StrightLineMotion::timeExpend() const { + if(_accerlate_rates == 0.0) + return _length_total / _speed_motion; + auto sqrt_v = std::sqrt(_speed_motion * _speed_motion + 2 * _accerlate_rates * _length_total); return (sqrt_v - _speed_motion) / _accerlate_rates; } diff --git a/SimsWorld.sln b/SimsWorld.sln index 99d89dd..5a45a73 100644 --- a/SimsWorld.sln +++ b/SimsWorld.sln @@ -31,6 +31,15 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MessageBasic", "MessageBasi EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "StandardGlobe", "StandardGlobe\StandardGlobe.vcxproj", "{C4A07990-233A-46F5-A3A0-48544CABA594}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "TestProject", "TestProject\TestProject.vcxproj", "{843BF0C3-E18F-42FA-AA90-894AD0E116A2}" + ProjectSection(ProjectDependencies) = postProject + {20CFC220-4F5B-4D39-97FA-74C70768F1A7} = {20CFC220-4F5B-4D39-97FA-74C70768F1A7} + {46E3FBA5-6775-4638-AE2E-935675D3B9CE} = {46E3FBA5-6775-4638-AE2E-935675D3B9CE} + {C4A07990-233A-46F5-A3A0-48544CABA594} = {C4A07990-233A-46F5-A3A0-48544CABA594} + {DF15B899-B9AE-4EBE-8FCC-436C9DB6CEF0} = {DF15B899-B9AE-4EBE-8FCC-436C9DB6CEF0} + {E1104048-F35B-40B7-995C-0320E689BF09} = {E1104048-F35B-40B7-995C-0320E689BF09} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -57,6 +66,10 @@ Global {C4A07990-233A-46F5-A3A0-48544CABA594}.Debug|x64.Build.0 = Debug|x64 {C4A07990-233A-46F5-A3A0-48544CABA594}.Release|x64.ActiveCfg = Release|x64 {C4A07990-233A-46F5-A3A0-48544CABA594}.Release|x64.Build.0 = Release|x64 + {843BF0C3-E18F-42FA-AA90-894AD0E116A2}.Debug|x64.ActiveCfg = Debug|x64 + {843BF0C3-E18F-42FA-AA90-894AD0E116A2}.Debug|x64.Build.0 = Debug|x64 + {843BF0C3-E18F-42FA-AA90-894AD0E116A2}.Release|x64.ActiveCfg = Release|x64 + {843BF0C3-E18F-42FA-AA90-894AD0E116A2}.Release|x64.Build.0 = Release|x64 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/StandardGlobe/standardglobe.cpp b/StandardGlobe/standardglobe.cpp index 62ae04e..d553f76 100644 --- a/StandardGlobe/standardglobe.cpp +++ b/StandardGlobe/standardglobe.cpp @@ -2,7 +2,7 @@ #include #include -double StandardGlobe::_radius_m = 65000000; +double StandardGlobe::_radius_m = 6500000; StandardGlobe::StandardGlobe() { @@ -32,6 +32,8 @@ LonLatAlt StandardGlobe::ecefToLLA(const ECEFv3d& v) rad_lon *= -1; auto rx = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos + v._z_pos * v._z_pos); auto rad_lat = asin(v._z_pos / rx); + if(v._z_pos < 0) + rad_lat *= -1; LonLatAlt ret; ret._lon_deg = rad2d(rad_lon); @@ -41,7 +43,7 @@ LonLatAlt StandardGlobe::ecefToLLA(const ECEFv3d& v) return ret; } -void StandardGlobe::getDistanceWithTargetLLA(const LonLatAlt& base, const LonLatAlt& target, double& dist, double& azi) +void StandardGlobe::getDistanceWithLLA(const LonLatAlt& base, const LonLatAlt& target, double& dist, double& azi) { // ecefʸÁ¿ auto vec_base = llaToEcef(base).getVec3(); @@ -72,18 +74,18 @@ void StandardGlobe::getDistanceWithTargetLLA(const LonLatAlt& base, const LonLat } #include -void StandardGlobe::getTargetLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target) +void StandardGlobe::getLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target) { - auto ecef_bvec = llaToEcef(base).getVec3().normalized(); - QQuaternion u0(-deg2a(azi), ecef_bvec); + auto ecef_basevec = llaToEcef(base).getVec3().normalized(); + QQuaternion u0 = QQuaternion::fromAxisAndAngle(ecef_basevec, -azi); // »ñÈ¡ÇÐÏß·½ÏòʸÁ¿ auto vec_hv = u0.rotatedVector(QVector3D(0, 0, 1)).normalized(); // »ñÈ¡·¨Ïß·½ÏòʸÁ¿ - auto vec_bv = QVector3D::crossProduct(ecef_bvec, vec_hv).normalized(); + auto vec_bv = QVector3D::crossProduct(ecef_basevec, vec_hv).normalized(); auto rad_between = dist / _radius_m; - QQuaternion u1(rad_between, vec_bv); + QQuaternion u1 = QQuaternion::fromAxisAndAngle(vec_bv, rad2d(rad_between)); auto ecef_t = u1.rotatedVector(llaToEcef(base).getVec3()); ECEFv3d sv; @@ -112,7 +114,7 @@ PolarPos StandardGlobe::getPolarWithLLA(const LonLatAlt& base, const LonLatAlt& // 180¡ãУÑé auto flag_vec = QVector3D::crossProduct(vec_hn, QVector3D(0, 0, 1)).normalized(); - if (vec_bn.z() * flag_vec.z() < 0) + if (flag_vec.z() < 0) angle = 2 * PI - angle; // ·µ»Ø·½Î»½Ç @@ -124,6 +126,12 @@ PolarPos StandardGlobe::getPolarWithLLA(const LonLatAlt& base, const LonLatAlt& auto cos_pitch = QVector3D::dotProduct(vec_hn, vec_btn); auto pit_deg = rad2d(acos(cos_pitch)); + auto axis_x = QVector3D::crossProduct(vec_hn, vec_btn).normalized(); + auto axis_flag = QVector3D::crossProduct(axis_x, vec_hn); + if (QVector3D::dotProduct(axis_flag, vec_bn) < 0) { + pit_deg *= -1; + } + PolarPos p; p._azimuth_deg = azi_deg; p._pitch_deg = pit_deg; @@ -133,17 +141,16 @@ PolarPos StandardGlobe::getPolarWithLLA(const LonLatAlt& base, const LonLatAlt& 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()); + auto ecef_bvec = llaToEcef(base).getVec3(); + auto u_azi = QQuaternion::fromAxisAndAngle(ecef_bvec.normalized(),-target._azimuth_deg); auto vec_ts = u_azi.rotatedVector(QVector3D(0, 0, 1)).normalized(); - auto axis_pitch = QVector3D::crossProduct(vec_ts, ecef_b.normalized()); - auto u_pit = QQuaternion(deg2a(target._pitch_deg), axis_pitch); + auto axis_pitch = QVector3D::crossProduct(vec_ts, ecef_bvec.normalized()); + auto u_pit = QQuaternion::fromAxisAndAngle(axis_pitch, target._pitch_deg); - auto r_shadow = target._dist_m * cos(deg2a(target._pitch_deg)); - auto ts_appv = u_pit.rotatedVector(vec_ts).normalized() * r_shadow; + auto ts_appv = u_pit.rotatedVector(vec_ts).normalized() * target._dist_m; - auto ecef_target = ecef_b + ts_appv; + auto ecef_target = ecef_bvec + ts_appv; ECEFv3d tt = ECEFv3d::fromVec3(ecef_target); return ecefToLLA(tt); diff --git a/StandardGlobe/standardglobe.h b/StandardGlobe/standardglobe.h index 3603301..6a86d35 100644 --- a/StandardGlobe/standardglobe.h +++ b/StandardGlobe/standardglobe.h @@ -2,7 +2,7 @@ #include "standardglobe_global.h" -#define PI 3.14159265354 +#define PI 3.14159265358979323846 #define deg2a(v) v / 180.0 * PI #define rad2d(v) v / PI * 180.0 @@ -82,7 +82,7 @@ public: /// /// /// - static void getDistanceWithTargetLLA(const LonLatAlt& base, const LonLatAlt& target, double& dist, double& azi); + static void getDistanceWithLLA(const LonLatAlt& base, const LonLatAlt& target, double& dist, double& azi); /// /// ¸ù¾ÝÁ½µãÖ®¼äµÄ´óµØÏß³¤¶ÈºÍb->t³õʼ·½Î»½Ç£¬¼ÆËãÄ¿±êLLA¶¨Î»×ø±ê /// @@ -90,7 +90,7 @@ public: /// /// /// - static void getTargetLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target); + static void getLLAWithDistance(const LonLatAlt& base, double dist, double azi, LonLatAlt& target); /// /// ͨ¹ýLLA×ø±ê£¬»ñÈ¡Ä¿±ê¼«×ø±ê /// diff --git a/TestProject/TestProject.vcxproj b/TestProject/TestProject.vcxproj new file mode 100644 index 0000000..3302cf4 --- /dev/null +++ b/TestProject/TestProject.vcxproj @@ -0,0 +1,110 @@ + + + + + Debug + x64 + + + Release + x64 + + + + {843BF0C3-E18F-42FA-AA90-894AD0E116A2} + QtVS_v304 + 10.0 + 10.0 + $(MSBuildProjectDirectory)\QtMsBuild + + + + Application + v143 + true + Unicode + + + Application + v143 + false + true + Unicode + + + + + + + 5.12.11_msvc2017_64 + core;gui + debug + + + 5.12.11_msvc2017_64 + core + release + + + + + + + + + + + + + + + + + $(SolutionDir)StandardGlobe;$(SolutionDir)ComponentBasic;$(SolutionDir)MessageBasic;$(SolutionDir)SimsBasic;$(IncludePath) + $(SolutionDir)$(Platform)\$(Configuration);$(LibraryPath) + + + + + + StandardGlobe.lib;MessageBasic.lib;ComponentBasic.lib;SimsBasic.lib;%(AdditionalDependencies) + + + + + true + Level3 + true + true + + + Console + true + + + + + true + Level3 + true + true + true + true + + + Console + false + true + true + + + + + + + + + + + + \ No newline at end of file diff --git a/TestProject/TestProject.vcxproj.filters b/TestProject/TestProject.vcxproj.filters new file mode 100644 index 0000000..0d52bb1 --- /dev/null +++ b/TestProject/TestProject.vcxproj.filters @@ -0,0 +1,31 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + qml;cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + qrc;rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + {99349809-55BA-4b9d-BF79-8FDBB0286EB3} + ui + + + {639EADAA-A684-42e4-A9AD-28FC9BCB8F7C} + ts + + + + + Source Files + + + + diff --git a/TestProject/TestProject.vcxproj.user b/TestProject/TestProject.vcxproj.user new file mode 100644 index 0000000..41a709a --- /dev/null +++ b/TestProject/TestProject.vcxproj.user @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/TestProject/main.cpp b/TestProject/main.cpp new file mode 100644 index 0000000..af5fce5 --- /dev/null +++ b/TestProject/main.cpp @@ -0,0 +1,47 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char* argv[]) +{ + QCoreApplication app(argc, argv); + + auto cmd = std::make_shared(); + cmd->_start_time = 0; + cmd->_speed_motion = 20; + cmd->_length_total = 200000; + + auto enti = std::make_shared(); + auto motion = std::make_shared(); + enti->append(motion); + auto box3d = std::make_shared(); + enti->append(box3d); + + auto immediate = std::make_shared(enti); + QList> out; + motion->execute(immediate, cmd, out); + + for (auto rst : out) + qDebug() << rst->_success_mark << rst->_reason_text; + + + auto total = cmd->timeExpend(); + for (auto timepoint = 0; timepoint < 10000; timepoint += 1) { + //qDebug() << cmd->posAtTimespan(timepoint); + + auto sync = std::make_shared(); + sync->_time_current = timepoint; + motion->execute(immediate, sync, out); + + qDebug() <_self_lla._lon_deg).arg(box3d->_self_lla._lat_deg).arg(box3d->_self_lla._alt_m); + //qDebug() << box3d->_self_posture._azimuth_deg << box3d->_self_posture._pitch_deg << box3d->_self_posture._roll_deg; + //qDebug() << timepoint << "========================"; + } + + return app.exec(); +}