pvt.h

Go to the documentation of this file.

#pragma once

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

namespace coordConv {

    class PVT {
    public:
        double pos, vel, t;

        explicit PVT(double pos, double vel, double t)
        :
            pos(pos),
            vel(vel),
            t(t)
        { };
        
        explicit PVT()
        :
            pos(DoubleNaN),
            vel(DoubleNaN),
            t(DoubleNaN)
        { };
        
        PVT copy() const {
            return PVT(pos, vel, t);
        }
        
        PVT copy(double t) const {
            return PVT(getPos(t), vel, t);
        }
        
        double getPos(double t) const {
            return pos + (vel * (t - this->t));
        }
        
        void invalidate(double t = DoubleNaN) {
            pos = DoubleNaN;
            vel = DoubleNaN;
            this->t = t;
        }
        
        bool isfinite() const {
            return std::isfinite(pos) &&  std::isfinite(vel) && std::isfinite(t);
        }

        PVT & operator+=(PVT const &rhs) {
            pos += rhs.getPos(t);
            vel += rhs.vel;
            return *this;
        }

        PVT & operator-=(PVT const &rhs) {
            pos -= rhs.getPos(t);
            vel -= rhs.vel;
            return *this;
        }

        PVT & operator+=(double const &rhs) {
            pos += rhs;
            return *this;
        }

        PVT & operator-=(double const &rhs) {
            pos -= rhs;
            return *this;
        }

        PVT & operator*=(double const &rhs) {
            pos *= rhs;
            vel *= rhs;
            return *this;
        }

        PVT & operator/=(double const &rhs) {
            pos /= rhs;
            vel /= rhs;
            return *this;
        }


        PVT operator+(PVT const &rhs) const {
            PVT retVal(*this);
            retVal += rhs;
            return retVal;
        }

        PVT operator-(PVT const &rhs) const {
            PVT retVal(*this);
            retVal -= rhs;
            return retVal;
        }

        PVT operator+(double const &rhs) const {
            PVT retVal(*this);
            retVal += rhs;
            return retVal;
        }

        PVT operator-(double const &rhs) const {
            PVT retVal(*this);
            retVal -= rhs;
            return retVal;
        }

        PVT operator*(double const &rhs) const {
            PVT retVal(*this);
            retVal *= rhs;
            return retVal;
        }

        PVT operator/(double const &rhs) const {
            PVT retVal(*this);
            retVal /= rhs;
            return retVal;
        }
        
        PVT operator-() const {
            return PVT(-pos, -vel, t);
        }

        bool operator==(PVT const &rhs) {
            return (this->pos == rhs.pos) && (this->vel == rhs.vel) && (this->t == rhs.t);
        }

        bool operator!=(PVT const &rhs) {
            return !operator==(rhs);
        }
        
        std::string __repr__() const;
        
        void setFromPair(double const posPair[2], double t, double deltaT, bool isAngle) {
            pos = posPair[0];
            if (isAngle) {
                vel = coordConv::wrapCtr(posPair[1] - posPair[0]) / deltaT;
            } else {
                vel = (posPair[1] - posPair[0]) / deltaT;
            }
            this->t = t;
        }
    };

    bool polarFromXY(PVT &r, PVT &theta, PVT const &x, PVT const &y, double tai);

    void xyFromPolar(PVT &x, PVT &y, PVT const &r, PVT const &theta, double tai);

    void rot2D(PVT &rotX, PVT &rotY, PVT const &x, PVT const &y, double ang, double tai);

    inline PVT wrapPos(
        PVT const &pvt  
    ) {
        PVT ret = pvt;
        ret.pos = coordConv::wrapPos(ret.pos);
        return ret;
    }

    inline PVT wrapCtr(
        PVT const &pvt  
    ) {
        PVT ret = pvt;
        ret.pos = coordConv::wrapCtr(ret.pos);
        return ret;
    }

    std::ostream &operator<<(std::ostream &os, PVT const &pvt);

}