DYTSrouce/src/entities/TrajectoryTraceComponent.cpp

#include "entities/TrajectoryTraceComponent.h"

#include <algorithm>

#include <osg/BlendFunc>
#include <osg/Depth>
#include <osg/Geode>
#include <osg/StateSet>
#include <osgEarth/GeoData>

#include "common/SpdLogger.h"
#include "entities/Entity.h"
#include "entities/PathComponent.h"
#include "scene/SceneContent.h"
#include "utils/StringUtils.h"
#include "workspace/WorkSpace.h"

TrajectoryTraceComponent::TrajectoryTraceComponent(SceneComponent* parent)
    : SceneComponent(parent) {
}

TrajectoryTraceComponent::~TrajectoryTraceComponent() {
}

std::string TrajectoryTraceComponent::GetTypeName() {
    return "TrajectoryTraceComponent";
}

bool TrajectoryTraceComponent::SetAttribute(const char* name, const char* value) {
    if (0 == strcmp(name, "color")) {
        SetColor(StringUtils::StringToVec4(value));
        return true;
    } else if (0 == strcmp(name, "lineWidth")) {
        SetLineWidth(static_cast<float>(atof(value)));
        return true;
    } else if (0 == strcmp(name, "sampleInterval")) {
        SetSampleInterval(atof(value));
        return true;
    } else if (0 == strcmp(name, "minMoveDistance")) {
        SetMinMoveDistance(atof(value));
        return true;
    } else if (0 == strcmp(name, "maxPoints")) {
        SetMaxPoints(atoi(value));
        return true;
    } else if (0 == strcmp(name, "tailDuration")) {
        SetTailDuration(atof(value));
        return true;
    } else if (0 == strcmp(name, "visible")) {
        SetTraceVisible(0 == strcmp(value, "true"));
        return true;
    } else if (0 == strcmp(name, "dashed")) {
        SetDashed(0 == strcmp(value, "true"));
        return true;
    } else if (0 == strcmp(name, "dashLength")) {
        SetDashLength(atof(value));
        return true;
    } else if (0 == strcmp(name, "gapLength")) {
        SetGapLength(atof(value));
        return true;
    } else if (0 == strcmp(name, "dashScrollSpeed")) {
        SetDashScrollSpeed(atof(value));
        return true;
    }

    return SceneComponent::SetAttribute(name, value);
}

bool TrajectoryTraceComponent::SaveAttribute(tinyxml2::XMLElement* element) {
    element->SetAttribute("color", StringUtils::Vec4ToString(color_).c_str());
    element->SetAttribute("lineWidth", std::to_string(lineWidth_).c_str());
    element->SetAttribute("sampleInterval", std::to_string(sampleInterval_).c_str());
    element->SetAttribute("minMoveDistance", std::to_string(minMoveDistance_).c_str());
    element->SetAttribute("maxPoints", maxPoints_);
    element->SetAttribute("tailDuration", std::to_string(tailDuration_).c_str());
    element->SetAttribute("visible", traceVisible_ ? "true" : "false");
    element->SetAttribute("dashed", dashed_ ? "true" : "false");
    element->SetAttribute("dashLength", std::to_string(dashLength_).c_str());
    element->SetAttribute("gapLength", std::to_string(gapLength_).c_str());
    element->SetAttribute("dashScrollSpeed", std::to_string(dashScrollSpeed_).c_str());
    return SceneComponent::SaveAttribute(element);
}

void TrajectoryTraceComponent::Begin() {
    SceneComponent::Begin();

    if (mt_.valid()) {
        RemoveRender();
    }

    sampleAccum_ = 0.0;
    elapsedTime_ = 0.0;
    hasLastSample_ = false;
    attachedToScene_ = false;
    ClearTrace();
}

void TrajectoryTraceComponent::Update(double dt) {
    SceneComponent::Update(dt);

    Entity* entity = GetEntity();
    if (nullptr == entity || nullptr == entity->GetTransform()) {
        return;
    }

    elapsedTime_ += dt;
    sampleAccum_ += dt;
    TrimExpiredPoints();

    const osg::Vec3d currentPos = entity->GetTransform()->GetLocation();
    if (!hasLastSample_) {
        osg::Vec3d initialPos = currentPos;
        if (PathComponent* pathComponent = entity->GetComponent<PathComponent>()) {
            pathComponent->GetFirstPathLocation(&initialPos);
        }
        EnsureAttachedToScene();
        if (!AppendPoint(initialPos)) {
            return;
        }
        if (!AppendPoint(currentPos)) {
            return;
        }
        lastSamplePos_ = currentPos;
        hasLastSample_ = true;
        sampleAccum_ = 0.0;
        return;
    }

    EnsureAttachedToScene();
    if (vertices_->empty()) {
        if (!AppendPoint(lastSamplePos_)) {
            return;
        }
        if (!AppendPoint(currentPos)) {
            return;
        }
    } else if (!UpdateTailPoint(currentPos)) {
        return;
    }

    if (sampleAccum_ < sampleInterval_) {
        return;
    }

    const osg::Vec3d delta = currentPos - lastSamplePos_;
    if (delta.length2() < minMoveDistance_ * minMoveDistance_) {
        return;
    }

    sampleAccum_ = 0.0;
    if (!AppendPoint(currentPos)) {
        return;
    }
    lastSamplePos_ = currentPos;
}

void TrajectoryTraceComponent::AddToRender() {
    InitializeGeometry();

    if (!attachedToScene_) {
        return;
    }

    if (mt_->getNumParents() > 0) {
        return;
    }
    AttachTraceToScene();
}

void TrajectoryTraceComponent::SetColor(const osg::Vec4& color) {
    color_ = color;
    UpdateStyle();
}

const osg::Vec4& TrajectoryTraceComponent::GetColor() const {
    return color_;
}

void TrajectoryTraceComponent::SetLineWidth(float width) {
    lineWidth_ = std::max(1.0f, width);
    UpdateStyle();
}

float TrajectoryTraceComponent::GetLineWidth() const {
    return lineWidth_;
}

void TrajectoryTraceComponent::SetSampleInterval(double interval) {
    const double newInterval = std::max(0.01, interval);
    if (sampleInterval_ == newInterval) {
        return;
    }

    sampleInterval_ = newInterval;
    // Make editor changes visible immediately instead of waiting for the old accumulated cadence.
    sampleAccum_ = 0.0;
}

double TrajectoryTraceComponent::GetSampleInterval() const {
    return sampleInterval_;
}

void TrajectoryTraceComponent::SetMinMoveDistance(double distance) {
    minMoveDistance_ = std::max(0.0, distance);
}

double TrajectoryTraceComponent::GetMinMoveDistance() const {
    return minMoveDistance_;
}

void TrajectoryTraceComponent::SetMaxPoints(int maxPoints) {
    maxPoints_ = std::max(2, maxPoints);
    if (vertices_.valid() && static_cast<int>(vertices_->size()) > maxPoints_) {
        const int overflow = static_cast<int>(vertices_->size()) - maxPoints_;
        vertices_->erase(vertices_->begin(), vertices_->begin() + overflow);
        drawArrays_->setCount(static_cast<GLsizei>(vertices_->size()));
        vertices_->dirty();
        geometry_->dirtyBound();
    }
}

int TrajectoryTraceComponent::GetMaxPoints() const {
    return maxPoints_;
}

void TrajectoryTraceComponent::SetTailDuration(double duration) {
    tailDuration_ = std::max(0.0, duration);
    TrimExpiredPoints();
}

double TrajectoryTraceComponent::GetTailDuration() const {
    return tailDuration_;
}

void TrajectoryTraceComponent::SetTraceVisible(bool visible) {
    traceVisible_ = visible;
    if (geode_.valid()) {
        geode_->setNodeMask(traceVisible_ ? 0xffffffff : 0x0);
    }
}

bool TrajectoryTraceComponent::IsTraceVisible() const {
    return traceVisible_;
}

void TrajectoryTraceComponent::SetDashed(bool dashed) {
    dashed_ = dashed;
    UpdateStyle();
}

bool TrajectoryTraceComponent::IsDashed() const {
    return dashed_;
}

void TrajectoryTraceComponent::SetDashLength(double length) {
    dashLength_ = std::max(1.0, length);
    UpdateStyle();
}

double TrajectoryTraceComponent::GetDashLength() const {
    return dashLength_;
}

void TrajectoryTraceComponent::SetGapLength(double length) {
    gapLength_ = std::max(1.0, length);
    UpdateStyle();
}

double TrajectoryTraceComponent::GetGapLength() const {
    return gapLength_;
}

void TrajectoryTraceComponent::SetDashScrollSpeed(double speed) {
    dashScrollSpeed_ = speed;
}

double TrajectoryTraceComponent::GetDashScrollSpeed() const {
    return dashScrollSpeed_;
}

void TrajectoryTraceComponent::ClearTrace() {
    InitializeGeometry();

    vertices_->clear();
    sampleTimes_.clear();
    drawArrays_->setCount(0);
    vertices_->dirty();
    geometry_->dirtyBound();
    if (geode_.valid()) {
        geode_->dirtyBound();
    }
    if (mt_.valid()) {
        mt_->dirtyBound();
    }
}

void TrajectoryTraceComponent::InitializeGeometry() {
    if (mt_.valid()) {
        return;
    }

    mt_ = new osg::MatrixTransform;
    geode_ = new osg::Geode;
    geometry_ = new osg::Geometry;
    vertices_ = new osg::Vec3Array;
    colors_ = new osg::Vec4Array;
    drawArrays_ = new osg::DrawArrays(GL_LINE_STRIP, 0, 0);
    lineWidthState_ = new osg::LineWidth(lineWidth_);
    lineStippleState_ = new osg::LineStipple(1, 0xFFFF);

    geometry_->setDataVariance(osg::Object::DYNAMIC);
    geometry_->setUseDisplayList(false);
    geometry_->setUseVertexBufferObjects(true);
    geometry_->setVertexArray(vertices_.get());
    geometry_->addPrimitiveSet(drawArrays_.get());

    colors_->push_back(color_);
    geometry_->setColorArray(colors_.get(), osg::Array::BIND_OVERALL);

    osg::StateSet* stateSet = geode_->getOrCreateStateSet();
    stateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    stateSet->setMode(GL_BLEND, osg::StateAttribute::ON);
    stateSet->setAttributeAndModes(lineWidthState_.get(), osg::StateAttribute::ON);
    stateSet->setAttributeAndModes(new osg::BlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA));
    stateSet->setAttributeAndModes(new osg::Depth(osg::Depth::LEQUAL, 0.0, 1.0, false));
    stateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
    stateSet->setAttributeAndModes(lineStippleState_.get(), osg::StateAttribute::OFF);

    geode_->addDrawable(geometry_.get());
    mt_->addChild(geode_.get());
    geode_->setNodeMask(traceVisible_ ? 0xffffffff : 0x0);
    UpdateStyle();
}

void TrajectoryTraceComponent::EnsureAttachedToScene() {
    InitializeGeometry();

    if (!attachedToScene_) {
        attachedToScene_ = true;
    }
}

bool TrajectoryTraceComponent::AppendPoint(const osg::Vec3d& geoPoint) {
    InitializeGeometry();

    osg::Vec3d worldPoint;
    if (!ConvertGeoPointToWorld(geoPoint, worldPoint)) {
        return false;
    }

    vertices_->push_back(worldPoint);
    sampleTimes_.push_back(elapsedTime_);
    if (static_cast<int>(vertices_->size()) > maxPoints_) {
        const int overflow = static_cast<int>(vertices_->size()) - maxPoints_;
        vertices_->erase(vertices_->begin(), vertices_->begin() + overflow);
        for (int i = 0; i < overflow && !sampleTimes_.empty(); ++i) {
            sampleTimes_.pop_front();
        }
    }

    drawArrays_->setCount(static_cast<GLsizei>(vertices_->size()));
    vertices_->dirty();
    geometry_->dirtyBound();
    if (geode_.valid()) {
        geode_->dirtyBound();
    }
    if (mt_.valid()) {
        mt_->dirtyBound();
    }

    if (attachedToScene_ && mt_->getNumParents() == 0) {
        AttachTraceToScene();
    }
    return true;
}

bool TrajectoryTraceComponent::UpdateTailPoint(const osg::Vec3d& geoPoint) {
    InitializeGeometry();

    if (!vertices_.valid() || vertices_->empty()) {
        return AppendPoint(geoPoint);
    }

    osg::Vec3d worldPoint;
    if (!ConvertGeoPointToWorld(geoPoint, worldPoint)) {
        return false;
    }

    vertices_->back() = worldPoint;
    if (!sampleTimes_.empty()) {
        sampleTimes_.back() = elapsedTime_;
    }
    drawArrays_->setCount(static_cast<GLsizei>(vertices_->size()));
    vertices_->dirty();
    geometry_->dirtyBound();
    if (geode_.valid()) {
        geode_->dirtyBound();
    }
    if (mt_.valid()) {
        mt_->dirtyBound();
    }

    if (attachedToScene_ && mt_->getNumParents() == 0) {
        AttachTraceToScene();
    }
    return true;
}

bool TrajectoryTraceComponent::ConvertGeoPointToWorld(const osg::Vec3d& geoPoint, osg::Vec3d& worldPoint) const {
    if (nullptr == g_srs_) {
        LOG_WARN("TrajectoryTraceComponent::ConvertGeoPointToWorld - g_srs_ is nullptr");
        return false;
    }

    osgEarth::GeoPoint point(g_srs_, geoPoint);
    if (!point.toWorld(worldPoint)) {
        LOG_WARN("TrajectoryTraceComponent::ConvertGeoPointToWorld - failed to convert geo point to world");
        return false;
    }
    return true;
}

void TrajectoryTraceComponent::AttachTraceToScene() {
    Entity* entity = GetEntity();
    if (nullptr == entity) {
        LOG_WARN("TrajectoryTraceComponent::AttachTraceToScene - entity is nullptr");
        return;
    }

    WorkSpace* workspace = entity->GetWorkspace();
    if (nullptr == workspace) {
        LOG_WARN("TrajectoryTraceComponent::AttachTraceToScene - workspace is nullptr");
        return;
    }

    OEScene* scene = workspace->GetActiveScene();
    if (nullptr == scene) {
        LOG_WARN("TrajectoryTraceComponent::AttachTraceToScene - scene is nullptr");
        return;
    }

    scene->AddToScene(mt_.get());
}

void TrajectoryTraceComponent::TrimExpiredPoints() {
    if (!vertices_.valid()) {
        return;
    }

    if (tailDuration_ <= 0.0) {
        if (!vertices_->empty()) {
            ClearTrace();
        }
        return;
    }

    bool removed = false;
    while (!sampleTimes_.empty() && (elapsedTime_ - sampleTimes_.front()) > tailDuration_) {
        sampleTimes_.pop_front();
        if (!vertices_->empty()) {
            vertices_->erase(vertices_->begin());
            removed = true;
        }
    }

    if (removed) {
        drawArrays_->setCount(static_cast<GLsizei>(vertices_->size()));
        vertices_->dirty();
        geometry_->dirtyBound();
        if (geode_.valid()) {
            geode_->dirtyBound();
        }
        if (mt_.valid()) {
            mt_->dirtyBound();
        }
    }
}

void TrajectoryTraceComponent::UpdateStyle() {
    if (colors_.valid()) {
        colors_->clear();
        colors_->push_back(color_);
        colors_->dirty();
        geometry_->dirtyBound();
    }

    if (lineWidthState_.valid()) {
        lineWidthState_->setWidth(lineWidth_);
    }

    if (lineStippleState_.valid() && geode_.valid()) {
        const double cycle = std::max(1.0, dashLength_ + gapLength_);
        double factorValue = cycle / std::max(1.0f, lineWidth_);
        if (factorValue < 1.0) {
            factorValue = 1.0;
        } else if (factorValue > 255.0) {
            factorValue = 255.0;
        }
        int factor = static_cast<int>(factorValue);
        int onBits = static_cast<int>(std::round(16.0 * dashLength_ / cycle));
        if (onBits < 1) {
            onBits = 1;
        } else if (onBits > 15) {
            onBits = 15;
        }
        unsigned short pattern = 0;
        for (int i = 0; i < 16; ++i) {
            if (i < onBits) {
                pattern |= static_cast<unsigned short>(1u << i);
            }
        }
        if (!dashed_) {
            pattern = 0xFFFF;
            geode_->getOrCreateStateSet()->setAttributeAndModes(lineStippleState_.get(), osg::StateAttribute::OFF);
        } else {
            geode_->getOrCreateStateSet()->setAttributeAndModes(lineStippleState_.get(), osg::StateAttribute::ON);
        }
        lineStippleState_->setFactor(factor);
        lineStippleState_->setPattern(pattern);
    }
}