Tools/GeneralParticleSystem/GeneralParticleSystem.h

Go to the documentation of this file.

/**
* @file GeneralParticleSystem.h
* 
* Template Class for a general particle System
* 
* @author <A href=mailto:robocup@ja-me.de>Matthias Meyer</A>
*/

#ifndef __GeneralParticleSystem_H__
#define __GeneralParticleSystem_H__

#define GPS_ShowGrid

#include <vector>
#include <list>
#include <algorithm>
#include "GPSParticle.h"
#include "Tools/Math/Vector2.h"

namespace GPS {

  typedef struct {
    int x;
    int y;
  } coord;

  template <class Particle> class GeneralParticleSystem {

    typedef struct {
      std::vector<Particle> v;
    } vc;


  private:
    std::vector<Particle> particleStore;
    int minX, minY, maxX, maxY;

    typedef struct {
      int x;
      int y;
      int count;
    } bestCellt;

    // local function to compare 2 bestCells
    static bool DFBestCellGreater(bestCellt b, bestCellt a) {
      return a.count <= b.count;
    }

    typedef std::list<bestCellt> LISTSTR;

    /**
    * Find the min / max values of the coordinates in the particle System
    */
    void findMinMaxValuesInParticleStore() {
      unsigned int i = 0;
      for (i = 0; i < particleStore.size(); i++) {
        if (minX > particleStore[i].pose.translation.x)
          minX = (int)particleStore[i].pose.translation.x;
        if (minY > particleStore[i].pose.translation.y)
          minY = (int)particleStore[i].pose.translation.y;
        if (maxX < particleStore[i].pose.translation.x)
          maxX = (int)particleStore[i].pose.translation.x;
        if (maxY < particleStore[i].pose.translation.y)
          maxY = (int)particleStore[i].pose.translation.y;
      }
    }

  public:
    // Super ugly typedef hack :(
    typedef std::vector<Particle> VOP;
    typedef std::vector<VOP> VOVOP;
    typedef std::vector<VOVOP> VOVOVOP;


    /**
    * Default constructor
    */
    GeneralParticleSystem() {
      minX = 0;
      maxX = 0;
      minY = 0;
      maxY = 0;
    }

    /**
    * Default destructor
    */
    virtual ~GeneralParticleSystem() {
    }


    /**
    * Return maximal X Value of the particles
    */
    int getMaxX() {
      return maxX;
    }

    /**
    * Return maximal Y Value of the particles
    */
    int getMaxY() {
      return maxY;
    }

    /**
    * Return minimal X Value of the particles
    */
    int getMinX() {
      return minX;
    }

    /**
    * Return minimal X Value of the particles
    */
    int getMinY() {
      return minY;
    }

    /**
    * Add a Particle to the ParticleStore
    */
    void addParticle(Particle p) {
      particleStore.push_back(p);
      if (maxX < p.pose.translation.x)
        maxX = (int)p.pose.translation.x;
      if (maxY < p.pose.translation.y)
        maxY = (int)p.pose.translation.y;
      if (minX > p.pose.translation.x)
        minX = (int)p.pose.translation.x;
      if (minY > p.pose.translation.y)
        minY = (int)p.pose.translation.y;
    }

    /**
    * Get a Particle from the ParticleStore
    */
    Particle& getParticle(int i) {
      return particleStore[i];
    }

    /**
    * Get a particle with a specified ID
    */
    Particle& getParticleByID(unsigned long id) {
      for (int i = 0; i < particleStore.size; i++) {
        if (particleStore[i].getID() == id) {
          return particleStore[i];
        }
      }
      return null;
    }

    /**
    * Remove a particle from the ParticleStore
    */
    void removeParticle(int i) {
      for (unsigned int j = i; j < particleStore.size()-1; j++) {
        particleStore[j] = particleStore[j+1];
      }
      particleStore.pop_back();
      findMinMaxValuesInParticleStore();
    }

    /**
    * Return the number of particles in the ParticleStore
    */
    int getNumOfParticles() {
      return particleStore.size();
    }

    /**
    * Return the particle in the store by the [] Operator
    */
    inline Particle& operator[] (int i)
    {
      return getParticle(i);
    }

    /**
    * Return the clusterPoints of the Particles in the System
    */
    std::vector< std::vector<Particle> > getCluster(unsigned int numberOfClusterToFind = 4, int densityX = 20, int densityY = 20) {
      // Variables
      std::vector< std::vector<Particle> >  result;
      int i,j;
      int xPos,yPos;
      int gridsizeX = (int)((maxX - minX) / densityX);
      int gridsizeY = (int)((maxY - minY) / densityY);

      VOVOVOP grid;
      std::vector<bestCellt> bestCells;

      // Resize the grid as needed
      grid.resize(densityX + 1);
      for (i = 0; i <= densityX; i++) {
        grid[i].resize(densityY + 1);
      }

#ifdef GPS_ShowGrid
      // Show the Grid
      for (i = 0; i <= densityX; i++) {
        LINE(playerModelField,minX,(i*gridsizeY) + minY, maxX,((i)*gridsizeY) + minY,3,Drawings::ps_solid,Drawings::red);
        LINE(playerModelField,(i*gridsizeX) + minX ,minY,((i)*gridsizeX) + minX ,maxY,3,Drawings::ps_solid,Drawings::red);
      }
#endif

      // Assign particles in the grid
      for (i = 0; i < getNumOfParticles(); i++) {
        xPos = (int)((particleStore[i].getPos().x - minX)/gridsizeX);
        yPos = (int)((particleStore[i].getPos().y - minY)/gridsizeY);
        //        Particle xyz = getParticle(i);
        grid[xPos][yPos].push_back(getParticle(i));
      }
      
      for (i = 0; i < densityX; i++) {
        for (j = 0; j < densityY; j++) {
          if (grid[i][j].size() > 0) {
            bestCellt c;
            c.count = grid[i][j].size();
            c.x = i;
            c.y = j;
            bestCells.push_back(c);
          }
        }
      }

      std::sort(bestCells.begin(),bestCells.end(),DFBestCellGreater);
      for (i = 0; (((unsigned int)i < bestCells.size()) && ((unsigned int)i < numberOfClusterToFind)); i++) {
        result.push_back(grid[bestCells[i].x][bestCells[i].y]);
      }

//      result.push_back((grid[c.x][c.y]));
      return result;
    }
  };
}
#endif

---