MultiRegCamStr.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



#ifndef __MULTI_REG_CAM_STR_H
#define __MULTI_REG_CAM_STR_H

#include <MIPMatrix.h>
#include "time.h"
#include <fstream>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <R2.h>
#include <S1.h>
#include <SE2.h>

namespace MipBaselib {

    class TriaMatrix : public MIPMatrix {
    public:


        TriaMatrix(int rows, int cols) : MIPMatrix(rows, cols) {
        };


        TriaMatrix() : MIPMatrix() {
        };
        /*
        TriaMatrix growDim(TriaMatrix &matrix){
             TriaMatrix _matrixRis(matrix.rows+1,matrix.cols+1);
             for(int i=0;i<matrix.rows;i++)
                 for(int j=0;j<matrix.cols;j++)
                    _matrixRis.elms[i][j]=matrix.elms[i][j];
             return _matrixRis;
         }
         */

        void growDim(TriaMatrix* &matrix);
    };

}

namespace MipAlgorithms {

template <class T>
    class ObjId {
    private:
        vector<T> _obj;
        int _id;
    public:


        ObjId() {
            _id = -1;
            _obj.clear();
        }


        ObjId(T obj, int id) {
            _id = id;
            _obj.push_back(obj);
        }


        ObjId(vector<T> obj, int id) {
            _id = id;
            _obj.clear();
            _obj = obj;
        }


        ~ObjId() {

           _id = -1;
           _obj.clear();
            /*delete &_id;
            delete &_obj;*/
        }


        ObjId & operator=(const ObjId& rhs) {
            if (this != &rhs) { // Not necessary in this case but it is useful to don't forget it
                _id = rhs._id;
                _obj = rhs._obj;
            }
            return (*this);
        }


        vector<T> getObj() {
            return _obj;
        }


        int getId() {
            return _id;
        }


        void setObj(T obj) {
            _obj.push_back(obj);
        }


        void setObj(vector<T> obj, int id) {
            _obj = obj;
            _id = id;
        }

        void setObj(vector<T> obj) {
            _obj = obj;
        }


        void setObj(T obj, int id) {
            _obj.push_back(obj);
            _id = id;
        }


        void setId(int id) {
            _id = id;
        }

    };
    //Creating three classes based on the template
    typedef PosiFeature PosId;

    /*class PosId : public PosiFeature{
        private: Angle _ori;
        public: PosId() : PosiFeature() {
                };

                PosId(Position pos, int id) : PosiFeature(pos,id) {
                };

                void setOri(Decimal ori){
                    _ori = ori;
                }

                Angle getOri(){
                    return _ori;
                }

    };*/

    //typedef Pose BearId;

    typedef ObjId<Angle> AngleId;

    class AngleIdTime : public AngleId {
        private:
            long int _ttSec;
            long int _ttUSec;
        public:
            AngleIdTime(){};

            AngleIdTime(Angle angle,int id,long int ttSec, long int ttUSec);

            ~AngleIdTime();

            void setTimeSec(long int sec);

            void setTimeUSec(long int usec);

            void setTime(long int sec, long int usec);

            long int getTimeSec();

            long int getTimeUSec();

            // Check if the time in input is before or later the actual one
            bool overTime(long int sec, long int usec);

            bool underTime(long int sec, long int usec);

            Decimal getTime();

            bool underTime(Decimal sec);

            bool overTime(Decimal sec);

    };

    class InterPoints {
    private:
        vector<Position> _xy;

        //int _nPoints;

    public:
        InterPoints();

        InterPoints(Position xy);

        InterPoints(vector<Position> xy);

        InterPoints(Decimal x, Decimal y);

        ~InterPoints();

        int getSize();

        InterPoints& operator=(const InterPoints& rhs);

        void insertPoint(Position xy);

        void deletePoint(int pos);

        void deleteLastPoint();

        void insertPoints(vector<Position> xy);

        void insertPoint(Position ip,int pos);

        Position getPoint(int pos);

        Position getLastPoint();

        string print();

    };

    class Triangle {
    private:

        PosId *_firstVertex;
        PosId *_secondVertex;
        PosId *_thirdVertex;
        InterPoints *_interPoints3;
        InterPoints *_interPoints2;
        Angle _oriTwo;
        Angle _oriThree;
        Angle _oriTwoOne;
        Angle _oriTwoThree;
        Angle _oriThreeOne;
        Angle _oriThreeTwo;
    Angle _ori2;
    Angle _ori3;
    
        //bool _meIn;

    public:

        Triangle();

        Triangle(Position xy1, int id1, Position xy2, int id2, Position xy3, int id3);


        ~Triangle();


        vector<PosId> getVertex();

        PosId* getFirstVertex();

        PosId* getSecondVertex();

        PosId* getThirdVertex();

        PosId* getVertexByPos(int pos);

        PosId* getVertexByID(int id);

        // Pay attention: it could be a rototranslation, not only translation!
        void setMyIdVertex(int myId, Decimal &aSide, Decimal &bSide, Decimal &cSide);

        bool isAVertex(Position pos, Decimal thr);

        void getIds(vector<int> &ris);

        // To give it side-effect, type a & after InterPoints
        InterPoints getInter3();

        // / \brief Returns the intersection by three vector in edit mode
        InterPoints* setInter3();

        Position getInter3At(int i);


        int getInter3Size();


        InterPoints getInter2();


        int getInter2Size();


        void deleteDupPoint2(int pos);


        void deleteDupPoint3(int pos);


        void insertPoints3(vector<Position> iPoints);


        void insertPoint3(Position iPoint);


        void insertPoints2(vector<Position> iPoints);


        void insertPoint2(Position iPoint);
        /*
        void setPos(PosId posId,int pos){
                if(pos==0)
                    (*_firstVertex).setObj(posId.getObj(),posId.getId());
                if(pos==1)
                    (*_secondVertex).setObj(posId.getObj(),posId.getId());
                if(pos==2)
                    (*_thirdVertex).setObj(posId.getObj(),posId.getId());
                (*_firstVertex).
        }
         */

        bool checkId(int myId);

        bool sameIds(Triangle* tria);


        bool setOri(Angle oriTwo, Angle oriThree, Angle oriTwoF, Angle oriTwoS, Angle oriThreeF, Angle oriThreeS, Decimal projToll);

        bool setOriOpt(Angle oriTwo, Angle oriThree, Angle oriTwoF, Angle oriTwoS, Angle oriThreeF, Angle oriThreeS, Decimal projToll, Decimal noise=0.0);


        Angle getOriTwo();


        Angle getOriThree();


        Angle getOriTwoThree();


        Angle getOriTwoOne();


        Angle getOriThreeOne();


        Angle getOriThreeTwo();

        Angle getOriByPos(int pos);



        string print();

        Triangle& operator=(Triangle &A);

        bool operator==(Triangle &A) const;

        /*Triangle & operator=(const Triangle& rhs) {
            if (this != &rhs) { // Not necessary in this case but it is useful to don't forget it
                _firstVertex = rhs._firstVertex;
                _secondVertex = rhs._secondVertex;
                _thirdVertex = rhs._thirdVertex;
                _interPoints2 = rhs._interPoints2;
                _interPoints3 = rhs._interPoints3;
            }
            return (*this);
        }*/

    };

    class PolFusion {
    private:
        vector<Triangle*> _triangles;
        //TriaMatrix* _polMatrix;
        vector<PosId> _vertex;
        vector<Angle> _ori;
        InterPoints _intThree;
        vector<Triangle*> _triangleQueue;
        vector<MIPMatrix> _rotoTrasl;
        vector<Decimal> _scaleF;

    public:
        PolFusion();

        PolFusion(Triangle* tria);

        ~PolFusion();

        Triangle* getTriangle(int pos);

        void setTriangle(int pos,Triangle* tria);

        void deleteTriangle(int pos);

        void deleteLastTriangle();

        void deleteBifConflicts(int pos,int posAt, int idNew);


        //void setMatrix(int id1, int id2, int pos);


        int getSize();

        void insertVertex(PosId vertex);

        PosId getVertex(int pos);

        void setVertex(int pos,PosId vertex);

        void setQueue(Triangle* queue);

        int queueSize();

        void deleteQueueTriangle(int pos);

        void deleteQueueLastTriangle();

        void setScale(int i, Decimal scale);

        void setScale(Decimal scale);

        Decimal getScale(int i);

        void deleteScaleF(int pos);

        void deleteLastScaleF();

        void setRotoTrasl(int pos, MIPMatrix rt);

        void deleteRotoTrasl(int pos);

        void deleteLastRotoTrasl();

        void insertOri(Decimal ori);

        Decimal getOri(int pos);

        void setOri(int pos,Angle ori);

        void deleteOri(int pos);

        void deleteLastOri();

        void insertTriangle(Triangle *tria, int myId, int id1, int id2);

        PosId* searchVertex(int id);

        PosId* searchVertex(int id,int &posAt);

        void deleteVertex(int pos);

        void deleteLastVertex();

        int searchBif(PosId vertex,int &posAt,Decimal thr);

        void insertPoints(Triangle* tria);

        int getNInThree(int pos);

        Position getIntThree(int pos);

        Position getLastIntThree();

        void deleteIntThree(int pos);

        void deleteLastIntThree();

        void insertPointIntThree(Position ip,int pos);

        bool expandPol(int myId, vector<PolFusion*> actualSol, vector<PolFusion*> &bifPolFusion,Decimal linThr);

        //bool checkBif(MultiRegCamSol* &mrcsols,PosId vertex,Decimal thr,int myId){
        bool checkBif(PosId vertex,Decimal thr,int myId);

        string print(int opt=0);

        PolFusion& operator=(PolFusion &A);

    };

}

#endif