codeboss
/
SimsWorld
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修正了半径矢量的计算公式

This commit is contained in:
codeboss 2025-06-22 10:34:20 +08:00
parent fa10045601
commit 5ea592f822
4 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ComponentBasic/SurfaceMotion.cpp
View File

@ -100,8 +100,8 @@ void SurfaceMotionPlugin::execute(std::shared_ptr<Immediate> map,
rst->reset(in->targetEntity(), in->sourceEntity()); rst->reset(in->targetEntity(), in->sourceEntity());
if (_current_cmd) { if (_current_cmd) {
// 机体坐标系推演 // 机体坐标系推演
auto pos_d3 = _current_cmd->posAtTimespan(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()); auto pos_p3 = _current_cmd->speedAtTimeSpan(in->_target_time - _current_cmd->startTimepoint());
QMatrix4x4 t; QMatrix4x4 t;
t.rotate(-_start_posture._azimuth_deg, QVector3D(0, 0, 1)); t.rotate(-_start_posture._azimuth_deg, QVector3D(0, 0, 1));

20
MessageBasic/motion_access.cpp
View File

@ -73,7 +73,7 @@ double StrightLineMotion::timeExpend() const
} }
#include <QVector3D> #include <QVector3D>
QVector3D StrightLineMotion::posAtTimespan(double time) const QVector3D StrightLineMotion::posAtTimeSpan(double time) const
{ {
auto length = _speed_motion * time + auto length = _speed_motion * time +
0.5 * _accerlate_rates * time * time; 0.5 * _accerlate_rates * time * time;
@ -81,7 +81,7 @@ QVector3D StrightLineMotion::posAtTimespan(double time) const
return QVector3D(0, length, 0); return QVector3D(0, length, 0);
} }
QVector3D StrightLineMotion::speedAtTimespan(double time) const QVector3D StrightLineMotion::speedAtTimeSpan(double time) const
{ {
auto vspeed = _speed_motion + _accerlate_rates * time; auto vspeed = _speed_motion + _accerlate_rates * time;
return QVector3D(0, vspeed, 0); return QVector3D(0, vspeed, 0);
@ -121,20 +121,20 @@ double HorizontalArcMotion::timeExpend() const
return arc_length / _speed_motion; 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 radius = std::abs(_center_point.x());
auto arc_length = _speed_motion * time; auto arc_length = _speed_motion * time;
auto _rotate_rads = arc_length / radius; 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); std::sin(_rotate_rads), 0);
return vec * radius + QVector3D(_center_point); 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)); auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(0, 0, 1));
return speed_nv.normalized() * _speed_motion; return speed_nv.normalized() * _speed_motion;
} }
@ -185,20 +185,20 @@ double VerticalArcMotion::timeExpend() const
return arc_length / _speed_motion; 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 radius = std::abs(_center_point.z());
auto arc_length = _speed_motion * time; auto arc_length = _speed_motion * time;
auto rotate_rads = arc_length / radius; auto rotate_rads = arc_length / radius;
QVector3D vec_r(0, std::sin(rotate_rads), 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; 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)); auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(1, 0, 0));
return speed_nv.normalized() * _speed_motion; return speed_nv.normalized() * _speed_motion;
} }

16
MessageBasic/motion_access.h
View File

@ -43,13 +43,13 @@ struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
/// </summary> /// </summary>
/// <param name="time"></param> /// <param name="time"></param>
/// <returns>机体坐标系</returns> /// <returns>机体坐标系</returns>
virtual QVector3D posAtTimespan(double time) const = 0; virtual QVector3D posAtTimeSpan(double time) const = 0;
/// <summary> /// <summary>
/// 指定时间点的速度矢量 /// 指定时间点的速度矢量
/// </summary> /// </summary>
/// <param name="time"></param> /// <param name="time"></param>
/// <returns>机体坐标系</returns> /// <returns>机体坐标系</returns>
virtual QVector3D speedAtTimespan(double time) const = 0; virtual QVector3D speedAtTimeSpan(double time) const = 0;
}; };
/// <summary> /// <summary>
@ -113,8 +113,8 @@ struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
double startTimepoint() const override; double startTimepoint() const override;
double timeExpend() const override; double timeExpend() const override;
QVector3D posAtTimespan(double time) const override; QVector3D posAtTimeSpan(double time) const override;
QVector3D speedAtTimespan(double time) const override; QVector3D speedAtTimeSpan(double time) const override;
void recoveryFrom(const QJsonObject& obj) override; void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override; void saveTo(QJsonObject& obj) const override;
@ -146,8 +146,8 @@ struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
double startTimepoint() const override; double startTimepoint() const override;
double timeExpend() const override; double timeExpend() const override;
QVector3D posAtTimespan(double time) const override; QVector3D posAtTimeSpan(double time) const override;
QVector3D speedAtTimespan(double time) const override; QVector3D speedAtTimeSpan(double time) const override;
void recoveryFrom(const QJsonObject& obj) override; void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override; void saveTo(QJsonObject& obj) const override;
@ -178,8 +178,8 @@ struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
double startTimepoint() const override; double startTimepoint() const override;
double timeExpend() const override; double timeExpend() const override;
QVector3D posAtTimespan(double time) const override; QVector3D posAtTimeSpan(double time) const override;
QVector3D speedAtTimespan(double time) const override; QVector3D speedAtTimeSpan(double time) const override;
void recoveryFrom(const QJsonObject& obj) override; void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override; void saveTo(QJsonObject& obj) const override;

9
TestProject/main.cpp
View File

@ -11,10 +11,11 @@ int main(int argc, char* argv[])
{ {
QCoreApplication app(argc, argv); QCoreApplication app(argc, argv);
auto cmd = std::make_shared<StrightLineMotion>(); auto cmd = std::make_shared<HorizontalArcMotion>();
cmd->_start_time = 0; cmd->_start_time = 0;
cmd->_speed_motion = 20; cmd->_speed_motion = 20;
cmd->_length_total = 200000; cmd->_center_point = QVector3D(200000, 0, 0);
cmd->_rotate_deg = 90;
auto enti = std::make_shared<RtWsEntity>(); auto enti = std::make_shared<RtWsEntity>();
auto motion = std::make_shared<SurfaceMotionPlugin>(); auto motion = std::make_shared<SurfaceMotionPlugin>();
@ -31,8 +32,8 @@ int main(int argc, char* argv[])
auto total = cmd->timeExpend(); auto total = cmd->timeExpend();
for (auto timepoint = 0; timepoint < 10000; timepoint += 1) { for (auto timepoint = 0; timepoint < total; timepoint += 1) {
//qDebug() << cmd->posAtTimespan(timepoint); //qDebug() << cmd->posAtTimeSpan(timepoint);
auto sync = std::make_shared<SyncRequest>(); auto sync = std::make_shared<SyncRequest>();
sync->_time_current = timepoint; sync->_time_current = timepoint;