Trajectory.h

 
#pragma once
#include <vector>
#include "Vec3.h"
 
namespace CommonMath
{


    class Trajectory
    {
    public:

        Trajectory(std::vector<Vec3> coeffs, double startTime, double endTime)
          : _coeffs(coeffs), _startTime(startTime), _endTime(endTime)
        {
            assert(coeffs.size() == 6);
            assert(startTime <= endTime);
        }


        Trajectory(Vec3 c0, Vec3 c1, Vec3 c2, Vec3 c3, Vec3 c4, Vec3 c5, double startTime, double endTime)
          : _coeffs{c0, c1, c2, c3, c4, c5}, _startTime(startTime), _endTime(endTime)
        {
            assert(startTime <= endTime);
        }


        Vec3 GetVelocity(double t) const
        {
            assert(t >= _startTime);
            assert(t <= _endTime);
            return _coeffs[0] * t * t * t * t * t + _coeffs[1] * t * t * t * t + _coeffs[2] * t * t * t +
                   _coeffs[3] * t * t + _coeffs[4] * t + _coeffs[5];
        }


        Vec3 GetValue(double t) const
        {
            assert(t >= _startTime);
            assert(t <= _endTime);
            return _coeffs[0] * t * t * t * t * t + _coeffs[1] * t * t * t * t + _coeffs[2] * t * t * t +
                   _coeffs[3] * t * t + _coeffs[4] * t + _coeffs[5];
        }

        std::vector<Vec3> GetCoeffs()
        {
            return _coeffs;
        }

        double GetStartTime() const
        {
            return _startTime;
        }

        double GetEndTime() const
        {
            return _endTime;
        }


        std::vector<Vec3> GetDerivativeCoeffs() const
        {
            std::vector<Vec3> derivCoeffs;
            derivCoeffs.reserve(5);
            for (int i = 0; i < 5; i++)
            {
                derivCoeffs.push_back((5 - i) * _coeffs[i]);
            }
            return derivCoeffs;
        }


        Trajectory operator-(const Trajectory rhs)
        {
            double startTime = std::max(_startTime, rhs.GetStartTime());
            double endTime = std::min(_endTime, rhs.GetEndTime());
            return Trajectory(_coeffs[0] - rhs[0], _coeffs[1] - rhs[1], _coeffs[2] - rhs[2], _coeffs[3] - rhs[3],
                              _coeffs[4] - rhs[4], _coeffs[5] - rhs[5], startTime, endTime);
        }

        inline Vec3 operator[](int i) const
        {
            switch (i)
            {
                case 0:
                    return _coeffs[0];
                case 1:
                    return _coeffs[1];
                case 2:
                    return _coeffs[2];
                case 3:
                    return _coeffs[3];
                case 4:
                    return _coeffs[4];
                case 5:
                    return _coeffs[5];
                default:
                    // We should never get here (index out of bounds)
                    assert(false);
                    return Vec3();  // Returns vector of NaNs
            }
        }
 
    private:
        std::vector<Vec3> _coeffs;
        double _startTime, _endTime;
    };
 
}  // namespace CommonMath