Trajectory.cpp

Go to the documentation of this file.

#include "Trajectory.h"
 
 
using namespace armarx::trajectory;
 
Trajectory::Trajectory()
{
}
 
void
Trajectory::clear()
{
    tracks.clear();
}
 
VariantTrack&
Trajectory::addTrack(const TrackID& id)
{
    return tracks.emplace(id, VariantTrack{id}).first->second;
}
 
VariantTrack&
Trajectory::addTrack(const TrackID& id, const VariantTrack::UpdateFunc& updateFunc)
{
    return tracks.emplace(id, VariantTrack{id, updateFunc}).first->second;
}
 
void
Trajectory::addKeyframe(const TrackID& id, const VariantKeyframe& keyframe)
{
    (*this)[id].addKeyframe(keyframe);
}
 
void
Trajectory::addKeyframe(const TrackID& id, float time, const VariantValue& value)
{
    addKeyframe(id, VariantKeyframe{time, value});
}
 
void
Trajectory::update(float time, bool ignoreEmptyTracks)
{
    for (auto& it : tracks)
    {
        it.second.update(time, ignoreEmptyTracks);
    }
}
 
VariantTrack&
Trajectory::operator[](const TrackID& id)
{
    try
    {
        return tracks.at(id);
    }
    catch (const std::out_of_range&)
    {
        throw error::NoTrackWithID(id);
    }
}
 
const VariantTrack&
Trajectory::operator[](const TrackID& id) const
{
    try
    {
        return tracks.at(id);
    }
    catch (const std::out_of_range&)
    {
        throw error::NoTrackWithID(id);
    }
}
 
std::ostream&
armarx::trajectory::operator<<(std::ostream& os, const Trajectory& trajectory)
{
    os << "Trajectory with " << trajectory.tracks.size() << " tracks: ";
    for (const auto& [name, track] : trajectory.tracks)
    {
        os << "\n  - " << track;
    }
    return os;
}
 
namespace armarx
{
 
    auto
    trajectory::toUpdateFunc(std::function<void(float)> func) -> VariantTrack::UpdateFunc
    {
        return [func](VariantValue value) { func(std::get<float>(value)); };
    }
 
    auto
    trajectory::toUpdateFunc(std::function<void(const Eigen::MatrixXf&)> func)
        -> VariantTrack::UpdateFunc
    {
        return [func](VariantValue value) { func(std::get<Eigen::MatrixXf>(value)); };
    }
 
    auto
    trajectory::toUpdateFunc(std::function<void(const Eigen::Quaternionf&)> func)
        -> VariantTrack::UpdateFunc
    {
        return [func](VariantValue value) { func(std::get<Eigen::Quaternionf>(value)); };
    }
 
} // namespace armarx