ObjectSet.cpp

Go to the documentation of this file.

#include "ObjectSet.hpp"
#include "utility/GeometryHelper.hpp"

namespace ISM {
bool sortObjectPtrAscAlpha(const ObjectPtr& lhs, const ObjectPtr& rhs)
    {
        std::stringstream ssl;
        std::stringstream ssr;
        ssl << lhs;
        ssr << rhs;
        return ssl.str() < ssr.str();
}
    std::ostream& operator<<(std::ostream &strm, const ISM::ObjectSet &o) {
        strm<<"object set: ["<<std::endl;
        std::vector<ISM::ObjectPtr> os = o.objects;
        std::sort(os.begin(), os.end(), sortObjectPtrAscAlpha);
        for (ObjectPtr& object : os) {
            strm<<object<<std::endl;
        };
        return strm<<"]";
    }

    std::ostream& operator<<(std::ostream &strm, const ISM::ObjectSetPtr &o) {
        return strm<<(*o);
    }

    void ObjectSet::insert(ObjectPtr o) {
        if(!o)
    {
                std::cerr << "ObjectSet::insert not valid pointer" << std::endl;
                throw std::runtime_error("ObjectSet::insert not valid pointer");
        }
        o->pose->quat = GeometryHelper::normalize(o->pose->quat);
        objects.push_back(o);
    }
}