environment.hpp

/* $Id$ */

#ifndef SIMU_ENVIRONMENT_HPP
#define SIMU_ENVIRONMENT_HPP

#include <vector>

#include "jmath/jblas.hpp"
#include "jmath/random.hpp"
#include "geom/Declarations.hpp"

namespace jafar {
  namespace simu {

    using namespace jafar;

    class Robot;

    class Point {

    public:

      unsigned int id;
      jblas::vec x;
      
      Point(unsigned int id_, jblas::vec const& x_);

      inline double getX() const { return x(0); }
      inline double getY() const { return x(1); }
      inline double getZ() const { return x(2); }
      
    };

    std::ostream& operator <<(std::ostream& s, const Point& p_);

    class PointFactory {
    public:

      PointFactory(const jblas::vec& xMin_, const jblas::vec& xMax_, 
       unsigned int randomSeed=1);

      Point getRandomPoint(unsigned int idFactory_);

    protected: 
      jafar::jmath::MultiDimUniformDistribution random;
    };


    class Segment {

    public:

      unsigned int id;

      jblas::vec3 ext1;
      jblas::vec3 ext2;

      Segment(unsigned int id_, jblas::vec3 const& x1_, jblas::vec3 const& x2_);      
      inline double getX1() const { return ext1(0); }
      inline double getY1() const { return ext1(1); }
      inline double getZ1() const { return ext1(2); }
      inline double getX2() const { return ext2(0); }
      inline double getY2() const { return ext2(1); }
      inline double getZ2() const { return ext2(2); }

    };

    std::ostream& operator <<(std::ostream& s, Segment const& s_);

    class SegmentFactory {

    public:

      SegmentFactory(const jblas::vec& xMin_, const jblas::vec& xMax_,
         double lengthMin_, double lengthMax_,
         unsigned int randomSeed=1);

      Segment getRandomSegment(unsigned int idFactory_);

    protected:

      jafar::jmath::MultiDimUniformDistribution extremity;
      jafar::jmath::UniformDistribution directionTheta;
      jafar::jmath::UniformDistribution directionPhi;
      jafar::jmath::UniformDistribution length;

    };

    class Environment {

    public:

      jblas::vec xMin;
      jblas::vec xMax;

      typedef std::list<Point> PointsListType;
      PointsListType pointsList;

      PointsListType persistenPointsList;
      

      typedef std::list<Segment> SegmentsListType;
      SegmentsListType segmentsList;

      Environment(const jblas::vec& xMin_, const jblas::vec& xMax_,
      double segLengthMin_ = 1.0, double segLengthMax_ = 3.0,
      unsigned int randomSeedPt = 1, unsigned int randomSeedSeg = 1);

      ~Environment();

      void addRobot(Robot* robot_);

      unsigned int addPoint(jblas::vec const& feature_);
      unsigned int addPoint(double x, double y, double z);

      unsigned int addPersistentPoint(double x, double y, double z);

      unsigned int addSegment(double x1, double y1, double z1, double x2, double y2, double z2);

      void addPoints( const std::list<geom::Point3D>& points );
      void addSegments( const std::list<geom::Segment3D>& segments );
      void addRandomPoints(int nb_=1);
      void addRandomSegments(int nb_=1);

      void addAlignedPoints(const jblas::vec& x1_, const jblas::vec& x2_, int nbFeatures_, bool bothExt_=true);

      const std::list<Point>& getPointsList() const;
      const std::list<Segment>& getSegmentsList() const;
      std::list<geom::Segment3D> getSegments3DList() const;
      std::list<geom::Segment3D> getSegments3DNear(double x, double y, double z, double distance, const jblas::bounded_symmetric_matrix<double, 4 >* cov1 = 0, const jblas::bounded_symmetric_matrix<double, 4 >* cov2 = 0, const jblas::mat44* M = 0, jmath::NormalDistribution* normalDist = 0);
      std::string getPointsTclList() const;

      std::string getSegmentsTclList() const;

    protected:

      std::vector<Robot*> robots;

      PointFactory pointFactory;

      SegmentFactory segmentFactory;
    
      unsigned int idFactory;
      unsigned int generateId();

      friend std::ostream& operator <<(std::ostream& s, const Environment& e_);
    };

    std::ostream& operator <<(std::ostream& s, const Environment& e_);

  }
}

#endif // SIMU_ENVIRONMENT_HPP