From 5ea592f822516262e7ff383db8bdbf3a3e1d5896 Mon Sep 17 00:00:00 2001 From: codeboss <2422523675@qq.com> Date: Sun, 22 Jun 2025 10:34:20 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E4=BA=86=E5=8D=8A=E5=BE=84?= =?UTF-8?q?=E7=9F=A2=E9=87=8F=E7=9A=84=E8=AE=A1=E7=AE=97=E5=85=AC=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ComponentBasic/SurfaceMotion.cpp | 4 ++-- MessageBasic/motion_access.cpp | 20 ++++++++++---------- MessageBasic/motion_access.h | 16 ++++++++-------- TestProject/main.cpp | 9 +++++---- 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/ComponentBasic/SurfaceMotion.cpp b/ComponentBasic/SurfaceMotion.cpp index 745ccff..16c7802 100644 --- a/ComponentBasic/SurfaceMotion.cpp +++ b/ComponentBasic/SurfaceMotion.cpp @@ -100,8 +100,8 @@ void SurfaceMotionPlugin::execute(std::shared_ptr map, 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()); + 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)); diff --git a/MessageBasic/motion_access.cpp b/MessageBasic/motion_access.cpp index ac7a3dd..6bdecea 100644 --- a/MessageBasic/motion_access.cpp +++ b/MessageBasic/motion_access.cpp @@ -73,7 +73,7 @@ double StrightLineMotion::timeExpend() const } #include -QVector3D StrightLineMotion::posAtTimespan(double time) const +QVector3D StrightLineMotion::posAtTimeSpan(double time) const { auto length = _speed_motion * time + 0.5 * _accerlate_rates * time * time; @@ -81,7 +81,7 @@ QVector3D StrightLineMotion::posAtTimespan(double time) const return QVector3D(0, length, 0); } -QVector3D StrightLineMotion::speedAtTimespan(double time) const +QVector3D StrightLineMotion::speedAtTimeSpan(double time) const { auto vspeed = _speed_motion + _accerlate_rates * time; return QVector3D(0, vspeed, 0); @@ -121,20 +121,20 @@ double HorizontalArcMotion::timeExpend() const return arc_length / _speed_motion; } -QVector3D HorizontalArcMotion::posAtTimespan(double time) const +QVector3D HorizontalArcMotion::posAtTimeSpan(double time) const { 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(), + QVector3D vec(-std::cos(_rotate_rads) * radius / _center_point.x(), std::sin(_rotate_rads), 0); return vec * radius + QVector3D(_center_point); } -QVector3D HorizontalArcMotion::speedAtTimespan(double time) const +QVector3D HorizontalArcMotion::speedAtTimeSpan(double time) const { - auto dist_vec = (posAtTimespan(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; } @@ -185,20 +185,20 @@ double VerticalArcMotion::timeExpend() const return arc_length / _speed_motion; } -QVector3D VerticalArcMotion::posAtTimespan(double time) const +QVector3D VerticalArcMotion::posAtTimeSpan(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()); + -std::cos(rotate_rads) * radius / _center_point.z()); return _center_point + vec_r * radius; } -QVector3D VerticalArcMotion::speedAtTimespan(double time) const +QVector3D VerticalArcMotion::speedAtTimeSpan(double time) const { - auto dist_vec = (posAtTimespan(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; } diff --git a/MessageBasic/motion_access.h b/MessageBasic/motion_access.h index e4858b5..0977672 100644 --- a/MessageBasic/motion_access.h +++ b/MessageBasic/motion_access.h @@ -43,13 +43,13 @@ struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage { /// /// /// 机体坐标系 - virtual QVector3D posAtTimespan(double time) const = 0; + virtual QVector3D posAtTimeSpan(double time) const = 0; /// /// 指定时间点的速度矢量 /// /// /// 机体坐标系 - virtual QVector3D speedAtTimespan(double time) const = 0; + virtual QVector3D speedAtTimeSpan(double time) const = 0; }; /// @@ -113,8 +113,8 @@ struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand { double startTimepoint() const override; double timeExpend() const override; - QVector3D posAtTimespan(double time) const override; - QVector3D speedAtTimespan(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; @@ -146,8 +146,8 @@ struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand { double startTimepoint() const override; double timeExpend() const override; - QVector3D posAtTimespan(double time) const override; - QVector3D speedAtTimespan(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; @@ -178,8 +178,8 @@ struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand { double startTimepoint() const override; double timeExpend() const override; - QVector3D posAtTimespan(double time) const override; - QVector3D speedAtTimespan(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/TestProject/main.cpp b/TestProject/main.cpp index e0540a9..56e3a22 100644 --- a/TestProject/main.cpp +++ b/TestProject/main.cpp @@ -11,10 +11,11 @@ int main(int argc, char* argv[]) { QCoreApplication app(argc, argv); - auto cmd = std::make_shared(); + auto cmd = std::make_shared(); cmd->_start_time = 0; cmd->_speed_motion = 20; - cmd->_length_total = 200000; + cmd->_center_point = QVector3D(200000, 0, 0); + cmd->_rotate_deg = 90; auto enti = std::make_shared(); auto motion = std::make_shared(); @@ -31,8 +32,8 @@ int main(int argc, char* argv[]) auto total = cmd->timeExpend(); - for (auto timepoint = 0; timepoint < 10000; timepoint += 1) { - //qDebug() << cmd->posAtTimespan(timepoint); + for (auto timepoint = 0; timepoint < total; timepoint += 1) { + //qDebug() << cmd->posAtTimeSpan(timepoint); auto sync = std::make_shared(); sync->_time_current = timepoint;