pvajt.h

Go to the documentation of this file.

#pragma once

#include <string>
#include <iostream>
#include "coordConv/mathUtils.h"
#include "coordConv/pvt.h"

namespace tcc {

    class PVAJT {
    public:
        double pos, vel, accel, jerk, t;

        explicit PVAJT(double pos, double vel, double accel, double jerk, double t);
        
        explicit PVAJT(coordConv::PVT const &pvt);
        
        explicit PVAJT();
        
        PVAJT extrapolate(double t) const {
            double dt = t - this->t;
            return PVAJT(
                pos   + dt*(vel   + dt*(accel/2.0 + dt*jerk/6.0)),
                vel   + dt*(accel + dt*jerk/2.0),
                accel + dt*jerk,
                jerk,
                t);
        }
        
        bool isfinite() const {
            return std::isfinite(pos)
                && std::isfinite(vel)
                && std::isfinite(accel)
                && std::isfinite(jerk)
                && std::isfinite(t);
        }
        
        bool operator==(PVAJT const &rhs) {
            return (this->pos == rhs.pos)
                && (this->vel == rhs.vel)
                && (this->accel == rhs.accel)
                && (this->jerk == rhs.jerk)
                && (this->t == rhs.t);
        }

        bool operator!=(PVAJT const &rhs) {
            return !operator==(rhs);
        }
       
        std::string __repr__() const;
    };

    std::ostream &operator<<(std::ostream &os, PVAJT const &pvajt);

}