DYTSrouce/src/utils/TransformPath.cpp

#include "utils/TransformPath.h"

#include <QFile>
#include <QTextStream>
#include <QRegExp>

#include "common/SpdLogger.h"


TransformPath::TransformPath(QObject* parent /*= nullptr*/)
    : QObject(parent) {

}

TransformPath::TransformPath(std::vector<Transform> transforms, QObject* parent /*= nullptr*/)
    : QObject(parent)
    , transforms_(std::move(transforms)){

}

TransformPath::~TransformPath() {

}

TransformPath* TransformPath::LoadFromFile(const QString& path, QObject* parent) {
    LOG_INFO("Loading transform path from file: {}", path.toLocal8Bit().constData());
    QFile file(path);

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        LOG_WARN("Cannot open file for reading: path:{} error:{}",
            path.toLocal8Bit().constData(), file.errorString().toLocal8Bit().constData());
        return nullptr;
    }

    QTextStream in(&file);

    std::vector<Transform> transforms;

    std::vector<double> times;

    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;
        transform.GetLocation().set(longitude, latitude, altitude);
        transform.GetRotation().set(pitch, roll, heading);

        times.push_back(time);

        transforms.push_back(transform);
    }
    file.close();

    TransformPath* tp = new TransformPath(std::move(transforms), parent);
    tp->SetTimes(std::move(times));
    return tp;
}