diff --git a/src/utils/TransformPath.cpp b/src/utils/TransformPath.cpp index 1cbc23f0..46cb4540 100644 --- a/src/utils/TransformPath.cpp +++ b/src/utils/TransformPath.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "common/SpdLogger.h" @@ -36,31 +37,37 @@ TransformPath* TransformPath::LoadFromFile(const QString& path, QObject* parent) std::vector transforms; while (!in.atEnd()) { + QString line = in.readLine(); + if (line.trimmed().isEmpty()) { + continue; + } + + // 期望格式:时间 经度 纬度 高度 方位角(Heading) 俯仰角(Pitch) 翻滚角(Roll) + // 示例:0.700000 \t 119.500000 \t 24.500000 \t 1000.000000 \t 0.000000 \t 0.000000 \t 0.000000 + const QStringList parts = line.split(QRegExp("\\s+"), Qt::SkipEmptyParts); + if (parts.size() < 7) { + LOG_WARN("TransformPath: invalid line (need 7 numbers): {}", line.toStdString()); + return {}; + } + + bool ok = false; + // 解析各字段 + const double time = parts[0].toDouble(&ok); if (!ok) { LOG_WARN("invalid time: {}", parts[0].toStdString()); return {}; } + const double longitude= parts[1].toDouble(&ok); if (!ok) { LOG_WARN("invalid longitude: {}", parts[1].toStdString()); return {}; } + const double latitude = parts[2].toDouble(&ok); if (!ok) { LOG_WARN("invalid latitude: {}", parts[2].toStdString()); return {}; } + const double altitude = parts[3].toDouble(&ok); if (!ok) { LOG_WARN("invalid altitude: {}", parts[3].toStdString()); return {}; } + const double heading = parts[4].toDouble(&ok); if (!ok) { LOG_WARN("invalid heading: {}", parts[4].toStdString()); return {}; } + const double pitch = parts[5].toDouble(&ok); if (!ok) { LOG_WARN("invalid pitch: {}", parts[5].toStdString()); return {}; } + const double roll = parts[6].toDouble(&ok); if (!ok) { LOG_WARN("invalid roll: {}", parts[6].toStdString()); return {}; } + Transform transform; + // 位置映射:经度->x,纬度->y,高度->z + transform.GetLocation().set(longitude, latitude, altitude); + // 旋转向量采用代码库约定顺序(Pitch, Roll, Heading),与 OsgUtils::HPRToQuat/QuatToHPR 保持一致 + transform.GetRotation().set(pitch, roll, heading); - for (int i = 0; i < 3; ++i) { - QString line = in.readLine(); - bool ok; - double value = line.toDouble(&ok); - if (ok) { - transform.GetLocation()[i] = value; - } else { - LOG_WARN("Failed to convert line to double: {}", line.toStdString()); - return {}; - } - } - - for (int i = 0; i < 3; ++i) { - QString line = in.readLine(); - bool ok; - double value = line.toDouble(&ok); - if (ok) { - transform.GetRotation()[i] = value; - } else { - LOG_WARN("Failed to convert line to double: {}", line.toStdString()); - return {}; - } - } + // 当前 PathComponent 使用 WorkSpace 的 Timestep 作为时间轴,这里暂不保存 time 字段 + // 若未来需要按文件时间驱动,可扩展 TransformPath 结构以记录时间。 transforms.push_back(transform); }