Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/GeneralParticleSystem/GeneralParticleSystem.h

Go to the documentation of this file.

    00001 /**
    00002 * @file GeneralParticleSystem.h
    00003 *
    00004 * Template Class for a general particle System
    00005 *
    00006 * @author <A href=mailto:robocup@ja-me.de>Matthias Meyer</A>
    00007 */
    00008
    00009 #ifndef __GeneralParticleSystem_H__
    00010 #define __GeneralParticleSystem_H__
    00011
    00012 #define GPS_ShowGrid
    00013
    00014 #include <vector>
    00015 #include <list>
    00016 #include <algorithm>
    00017 #include "GPSParticle.h"
    00018 #include "Tools/Math/Vector2.h"
    00019
    00020 namespace GPS {
    00021
    00022   typedef struct {
    00023     int x;
    00024     int y;
    00025   } coord;
    00026
    00027   template <class Particle> class GeneralParticleSystem {
    00028
    00029     typedef struct {
    00030       std::vector<Particle> v;
    00031     } vc;
    00032
    00033
    00034   private:
    00035     std::vector<Particle> particleStore;
    00036     int minX, minY, maxX, maxY;
    00037
    00038     typedef struct {
    00039       int x;
    00040       int y;
    00041       int count;
    00042     } bestCellt;
    00043
    00044     // local function to compare 2 bestCells
    00045     static bool DFBestCellGreater(bestCellt b, bestCellt a) {
    00046       return a.count <= b.count;
    00047     }
    00048
    00049     typedef std::list<bestCellt> LISTSTR;
    00050
    00051     /**
    00052     * Find the min / max values of the coordinates in the particle System
    00053     */
    00054     void findMinMaxValuesInParticleStore() {
    00055       unsigned int i = 0;
    00056       for (i = 0; i < particleStore.size(); i++) {
    00057         if (minX > particleStore[i].pose.translation.x)
    00058           minX = (int)particleStore[i].pose.translation.x;
    00059         if (minY > particleStore[i].pose.translation.y)
    00060           minY = (int)particleStore[i].pose.translation.y;
    00061         if (maxX < particleStore[i].pose.translation.x)
    00062           maxX = (int)particleStore[i].pose.translation.x;
    00063         if (maxY < particleStore[i].pose.translation.y)
    00064           maxY = (int)particleStore[i].pose.translation.y;
    00065       }
    00066     }
    00067
    00068   public:
    00069     // Super ugly typedef hack :(
    00070     typedef std::vector<Particle> VOP;
    00071     typedef std::vector<VOP> VOVOP;
    00072     typedef std::vector<VOVOP> VOVOVOP;
    00073
    00074
    00075     /**
    00076     * Default constructor
    00077     */
    00078     GeneralParticleSystem() {
    00079       minX = 0;
    00080       maxX = 0;
    00081       minY = 0;
    00082       maxY = 0;
    00083     }
    00084
    00085     /**
    00086     * Default destructor
    00087     */
    00088     virtual ~GeneralParticleSystem() {
    00089     }
    00090
    00091
    00092     /**
    00093     * Return maximal X Value of the particles
    00094     */
    00095     int getMaxX() {
    00096       return maxX;
    00097     }
    00098
    00099     /**
    00100     * Return maximal Y Value of the particles
    00101     */
    00102     int getMaxY() {
    00103       return maxY;
    00104     }
    00105
    00106     /**
    00107     * Return minimal X Value of the particles
    00108     */
    00109     int getMinX() {
    00110       return minX;
    00111     }
    00112
    00113     /**
    00114     * Return minimal X Value of the particles
    00115     */
    00116     int getMinY() {
    00117       return minY;
    00118     }
    00119
    00120     /**
    00121     * Add a Particle to the ParticleStore
    00122     */
    00123     void addParticle(Particle p) {
    00124       particleStore.push_back(p);
    00125       if (maxX < p.pose.translation.x)
    00126         maxX = (int)p.pose.translation.x;
    00127       if (maxY < p.pose.translation.y)
    00128         maxY = (int)p.pose.translation.y;
    00129       if (minX > p.pose.translation.x)
    00130         minX = (int)p.pose.translation.x;
    00131       if (minY > p.pose.translation.y)
    00132         minY = (int)p.pose.translation.y;
    00133     }
    00134
    00135     /**
    00136     * Get a Particle from the ParticleStore
    00137     */
    00138     Particle& getParticle(int i) {
    00139       return particleStore[i];
    00140     }
    00141
    00142     /**
    00143     * Get a particle with a specified ID
    00144     */
    00145     Particle& getParticleByID(unsigned long id) {
    00146       for (int i = 0; i < particleStore.size; i++) {
    00147         if (particleStore[i].getID() == id) {
    00148           return particleStore[i];
    00149         }
    00150       }
    00151       return null;
    00152     }
    00153
    00154     /**
    00155     * Remove a particle from the ParticleStore
    00156     */
    00157     void removeParticle(int i) {
    00158       for (unsigned int j = i; j < particleStore.size()-1; j++) {
    00159         particleStore[j] = particleStore[j+1];
    00160       }
    00161       particleStore.pop_back();
    00162       findMinMaxValuesInParticleStore();
    00163     }
    00164
    00165     /**
    00166     * Return the number of particles in the ParticleStore
    00167     */
    00168     int getNumOfParticles() {
    00169       return particleStore.size();
    00170     }
    00171
    00172     /**
    00173     * Return the particle in the store by the [] Operator
    00174     */
    00175     inline Particle& operator[] (int i)
    00176     {
    00177       return getParticle(i);
    00178     }
    00179
    00180     /**
    00181     * Return the clusterPoints of the Particles in the System
    00182     */
    00183     std::vector< std::vector<Particle> > getCluster(unsigned int numberOfClusterToFind = 4, int densityX = 20, int densityY = 20) {
    00184       // Variables
    00185       std::vector< std::vector<Particle> >  result;
    00186       int i,j;
    00187       int xPos,yPos;
    00188       int gridsizeX = (int)((maxX - minX) / densityX);
    00189       int gridsizeY = (int)((maxY - minY) / densityY);
    00190
    00191       VOVOVOP grid;
    00192       std::vector<bestCellt> bestCells;
    00193
    00194       // Resize the grid as needed
    00195       grid.resize(densityX + 1);
    00196       for (i = 0; i <= densityX; i++) {
    00197         grid[i].resize(densityY + 1);
    00198       }
    00199
    00200 #ifdef GPS_ShowGrid
    00201       // Show the Grid
    00202       for (i = 0; i <= densityX; i++) {
    00203         LINE(playerModelField,minX,(i*gridsizeY) + minY, maxX,((i)*gridsizeY) + minY,3,Drawings::ps_solid,Drawings::red);
    00204         LINE(playerModelField,(i*gridsizeX) + minX ,minY,((i)*gridsizeX) + minX ,maxY,3,Drawings::ps_solid,Drawings::red);
    00205       }
    00206 #endif
    00207
    00208       // Assign particles in the grid
    00209       for (i = 0; i < getNumOfParticles(); i++) {
    00210         xPos = (int)((particleStore[i].getPos().x - minX)/gridsizeX);
    00211         yPos = (int)((particleStore[i].getPos().y - minY)/gridsizeY);
    00212         //        Particle xyz = getParticle(i);
    00213         grid[xPos][yPos].push_back(getParticle(i));
    00214       }
    00215
    00216       for (i = 0; i < densityX; i++) {
    00217         for (j = 0; j < densityY; j++) {
    00218           if (grid[i][j].size() > 0) {
    00219             bestCellt c;
    00220             c.count = grid[i][j].size();
    00221             c.x = i;
    00222             c.y = j;
    00223             bestCells.push_back(c);
    00224           }
    00225         }
    00226       }
    00227
    00228       std::sort(bestCells.begin(),bestCells.end(),DFBestCellGreater);
    00229       for (i = 0; (((unsigned int)i < bestCells.size()) && ((unsigned int)i < numberOfClusterToFind)); i++) {
    00230         result.push_back(grid[bestCells[i].x][bestCells[i].y]);
    00231       }
    00232
    00233 //      result.push_back((grid[c.x][c.y]));
    00234       return result;
    00235     }
    00236   };
    00237 }
    00238 #endif