Pose.hpp

Go to the documentation of this file.

#pragma once

#include <sstream>
#include <string>
#include <ostream>
#include "Quaternion.hpp"
#include "Point.hpp"
#include "Serializable.hpp"

namespace ISM {

class Pose : public Serializable
{
public:
    PointPtr point;
    QuaternionPtr quat;

    Pose()
    {
        this->point = PointPtr(new Point(0, 0, 0));
        this->quat = QuaternionPtr(new Quaternion(1.0, 0, 0, 0));
    };

    Pose(const Pose& other)
    {
        this->point = PointPtr(new Point(*(other.point)));
        this->quat = QuaternionPtr(new Quaternion(*(other.quat)));
    };

    Pose(Point *p, Quaternion* q)
    {
        this->point = PointPtr(p);
        this->quat = QuaternionPtr(q);
    };

    Pose(Point *p, QuaternionPtr q) : quat(q)
    {
        this->point = PointPtr(p);
    };

    Pose(PointPtr p, Quaternion* q) : point(p)
    {
        this->quat = QuaternionPtr(q);
    };

    Pose(PointPtr p, QuaternionPtr quat) : point(p), quat(quat) {};

    virtual void serialize(std::ostream& strm) const;

    void convertPoseIntoFrame(const boost::shared_ptr<Pose>& pFrame, boost::shared_ptr<Pose>& pResult);

};
typedef boost::shared_ptr<Pose> PosePtr;

bool operator==(const PosePtr& pose1, const PosePtr& pose2);
bool operator==(const Pose& pose1, const Pose& pose2);

std::ostream& operator<<(std::ostream &strm, const ISM::Pose &p);
std::ostream& operator<<(std::ostream &strm, const ISM::PosePtr &p);
}