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

# Tools/PotentialFields/FormationObject.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file FormationObject.cpp
    00003 *
    00004 * Implementation of class FormationObject
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009
    00010 #include <cassert>
    00011 #include <cfloat>
    00012 #include "FormationObject.h"
    00013 #include "PfieldGeometry.h"
    00014
    00015
    00016
    00017 SingleFormation::SingleFormation()
    00018 {
    00019   geometricFormationObject.setField(SHAPE_FIELD);
    00020 }
    00021
    00022
    00023 bool SingleFormation::isStatic() const
    00024 {
    00025   std::vector < PotentialFieldsObject* >::const_iterator object;
    00026   for (object = objects.begin (); object != objects.end (); ++object)
    00027   {
    00028     if(!(*object)->isStatic())
    00029     {
    00030       return false;
    00031     }
    00032   }
    00033   return true;
    00034 }
    00035
    00036
    00037 PfVec SingleFormation::getPosition(const PfPose& robotPose)
    00038 {
    00039   robotPosition = robotPose.pos;
    00040   updateGeometry();
    00041   Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
    00042   PfVec nextPos;
    00043   double dist(polygon->distanceTo(PfPose(), robotPose.pos, nextPos));
    00044   if(dist != 0.0)
    00045   {
    00046     return nextPos;
    00047   }
    00048   else
    00049   {
    00050     return PfVec(0.0,0.0);
    00051   }
    00052 }
    00053
    00054
    00055 PfVec SingleFormation::getVec(const PfPose& otherPose)
    00056 {
    00057   robotPosition = otherPose.pos;
    00058   updateGeometry();
    00059   if(positionInsideFormation(otherPose.pos))
    00060   {
    00061     return PfVec(0.0,0.0);
    00062   }
    00063   else
    00064   {
    00065     return geometricFormationObject.computeAbsFieldVecAt(otherPose);
    00066   }
    00067 }
    00068
    00069
    00070 double SingleFormation::getCharge(const PfPose& otherPose)
    00071 {
    00072   robotPosition = otherPose.pos;
    00073   updateGeometry();
    00074   return geometricFormationObject.computeChargeAt(otherPose);
    00075 }
    00076
    00077
    00078 void SingleFormation::addObject(PotentialFieldsObject* object)
    00079 {
    00080   objects.push_back(object);
    00081 }
    00082
    00083
    00084 double SingleFormation::getDistanceToFormation(const PfPose& otherPose) const
    00085 {
    00086   PfieldGeometricObject* obj = geometricFormationObject.getGeometry();
    00087   PfPose dummyPose; PfVec dummyVec;
    00088   return obj->distanceTo(dummyPose, otherPose.pos, dummyVec);
    00089 }
    00090
    00091
    00092 SingleFormation* SingleFormation::copy() const
    00093 {
    00094   SingleFormation* singleFormation = new SingleFormation();
    00095   singleFormation->geometricFormationObject = geometricFormationObject;
    00096   singleFormation->objects = objects;
    00097   singleFormation->robotPosition = robotPosition;
    00098   singleFormation->priority = priority;
    00099   return singleFormation;
    00100 }
    00101
    00102
    00103 BetweenFormation::BetweenFormation()
    00104 {
    00105   Polygon* polygon = new Polygon();
    00106   geometricFormationObject.setGeometry(polygon);
    00107   delete polygon;
    00108 }
    00109
    00110
    00111 bool BetweenFormation::isActive() const
    00112 {
    00113   return (objects[0]->isActive() && objects[1]->isActive());
    00114 }
    00115
    00116
    00117 bool BetweenFormation::positionInsideFormation(const PfVec& p) const
    00118 {
    00119   Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
    00120   return (polygon->pointInside(p));
    00121 }
    00122
    00123
    00124 void BetweenFormation::updateGeometry()
    00125 {
    00126   Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
    00127   polygon->pts.clear();
    00128   std::vector < PotentialFieldsObject* >::const_iterator object;
    00129   for (object = objects.begin (); object != objects.end (); ++object)
    00130   {
    00131     (*object)->getAbsGeometry()->getPoints(polygon->pts);
    00132   }
    00133   if(polygon->pts.size() > 3)
    00134   {
    00135     reduceToConvexHullByWrapping(*polygon);
    00136   }
    00137   polygon->initRadiusOfCollisionCircle();
    00138 }
    00139
    00140
    00141 AmongFormation::AmongFormation()
    00142 {
    00143   Polygon* polygon = new Polygon();
    00144   geometricFormationObject.setGeometry(polygon);
    00145   delete polygon;
    00146 }
    00147
    00148
    00149 bool AmongFormation::isActive() const
    00150 {
    00151   int numberOfActiveObjects(0);
    00152   std::vector < PotentialFieldsObject* >::const_iterator object;
    00153   for (object = objects.begin(); object != objects.end(); ++object)
    00154   {
    00155     if((*object)->isActive())
    00156     {
    00157       ++numberOfActiveObjects;
    00158     }
    00159     if(numberOfActiveObjects >= 2)
    00160     {
    00161       return true;
    00162     }
    00163   }
    00164   return false;
    00165 }
    00166
    00167
    00168 void AmongFormation::updateGeometry()
    00169 {
    00170   Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
    00171   polygon->pts.clear();
    00172   std::vector < PotentialFieldsObject* >::const_iterator object;
    00173   for (object = objects.begin(); object != objects.end(); ++object)
    00174   {
    00175     (*object)->getAbsGeometry()->getPoints(polygon->pts);
    00176   }
    00177   if(polygon->pts.size() > 3)
    00178   {
    00179     reduceToConvexHullByWrapping(*polygon);
    00180   }
    00181   polygon->initRadiusOfCollisionCircle();
    00182 }
    00183
    00184
    00185 bool AmongFormation::positionInsideFormation(const PfVec& p) const
    00186 {
    00187   Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
    00188   return (polygon->pointInside(p));
    00189 }
    00190
    00191
    00192 RelativeFormation::RelativeFormation()
    00193 {
    00194   Line* line = new Line();
    00195   geometricFormationObject.setGeometry(line);
    00196   delete line;
    00197 }
    00198
    00199
    00200 bool RelativeFormation::isActive() const
    00201 {
    00202   return (objects[0]->isActive());
    00203 }
    00204
    00205
    00206 void RelativeFormation::updateGeometry()
    00207 {
    00208   Line* line = (Line*)geometricFormationObject.getGeometry();
    00209   PfPose objectPose(objects[0]->getPose());
    00210   line->p1 = objectPose.pos;
    00211   PfVec vecToP2(1.0,0.0);
    00212   vecToP2.rotate(angle);
    00213   if(coordinates == RELATIVE_FORMATION)
    00214   {
    00215     vecToP2.rotate(objectPose.rotation);
    00216   }
    00217   vecToP2 *= (line->p1.distanceTo(robotPosition)*2);
    00218   line->p2 = line->p1 + vecToP2;
    00219 }
    00220
    00221
    00222 SingleFormation* RelativeFormation::copy() const
    00223 {
    00224   RelativeFormation* relativeFormation = new RelativeFormation();
    00225   relativeFormation->geometricFormationObject = geometricFormationObject;
    00226   relativeFormation->objects = objects;
    00227   relativeFormation->robotPosition = robotPosition;
    00228   relativeFormation->angle = angle;
    00229   relativeFormation->coordinates = coordinates;
    00230   return relativeFormation;
    00231 }
    00232
    00233
    00234 BestFitFormation::BestFitFormation()
    00235 {
    00236 }
    00237
    00238
    00239 BestFitFormation::~BestFitFormation()
    00240 {
    00241   std::vector < SingleFormation* >::iterator formation;
    00242   for (formation = formations.begin ();
    00243        formation != formations.end (); ++formation)
    00244   {
    00245     delete (*formation);
    00246   }
    00247 }
    00248
    00249
    00250 bool BestFitFormation::isStatic() const
    00251 {
    00252   std::vector < SingleFormation* >::const_iterator formation;
    00253   for (formation = formations.begin ();
    00254        formation != formations.end (); ++formation)
    00255   {
    00256     if(!(*formation)->isStatic())
    00257     {
    00258       return false;
    00259     }
    00260   }
    00261   return true;
    00262 }
    00263
    00264
    00265 PfVec BestFitFormation::getPosition(const PfPose& robotPose)
    00266 {
    00267   int best(findBestFormation(robotPose));
    00268   if(best == -1)
    00269   {
    00270     return PfVec(0.0,0.0);
    00271   }
    00272   else
    00273   {
    00274     return formations[(unsigned int)best]->getPosition(robotPose);
    00275   }
    00276 }
    00277
    00278
    00279 PfVec BestFitFormation::getVec(const PfPose& otherPose)
    00280 {
    00281   int best(findBestFormation(otherPose));
    00282   if(best == -1)
    00283   {
    00284     return PfVec(0.0,0.0);
    00285   }
    00286   else
    00287   {
    00288     return formations[(unsigned int)best]->getVec(otherPose);;
    00289   }
    00290 }
    00291
    00292
    00293 double BestFitFormation::getCharge(const PfPose& otherPose)
    00294 {
    00295   int best(findBestFormation(otherPose));
    00296   if(best == -1)
    00297   {
    00298     return 0.0;
    00299   }
    00300   else
    00301   {
    00302     return formations[(unsigned int)best]->getCharge(otherPose);
    00303   }
    00304 }
    00305
    00306
    00307 bool BestFitFormation::isActive() const
    00308 {
    00309   std::vector < SingleFormation* >::const_iterator formation;
    00310   for (formation = formations.begin ();
    00311        formation != formations.end (); ++formation)
    00312   {
    00313     if((*formation)->isActive())
    00314     {
    00315       return true;
    00316     }
    00317   }
    00318   return false;
    00319 }
    00320
    00321
    00322 int BestFitFormation::findBestFormation(const PfPose& otherPose)
    00323 {
    00324   unsigned int best(0);
    00325   while(best < formations.size())
    00326   {
    00327     if(formations[best]->isActive())
    00328     {
    00329       break;
    00330     }
    00331     else
    00332     {
    00333       best++;
    00334     }
    00335   }
    00336   if(best < formations.size())
    00337   {
    00338     double selectionValue(0.0);
    00339     switch(bestFitSelection)
    00340     {
    00341     case SELECT_MIN_DISTANCE: selectionValue = formations[best]->getDistanceToFormation(otherPose);
    00342                               break;
    00343     case SELECT_MAX_GRADIENT: selectionValue = formations[best]->getVec(otherPose).length();
    00344                               break;
    00345     case SELECT_MIN_GRADIENT: selectionValue = formations[best]->getVec(otherPose).length();
    00346                               break;
    00347     case SELECT_MAX_PRIORITY: selectionValue = formations[best]->getPriority();
    00348                               break;
    00349     }
    00350     for(unsigned int i = best+1; i < formations.size(); i++)
    00351     {
    00352       if(formations[i]->isActive())
    00353       {
    00354         double val;
    00355         switch(bestFitSelection)
    00356         {
    00357         case SELECT_MIN_DISTANCE: val = formations[i]->getDistanceToFormation(otherPose);
    00358                                   if(val < selectionValue)
    00359                                   {
    00360                                     selectionValue = val;
    00361                                     best = i;
    00362                                   }
    00363                                   break;
    00364         case SELECT_MAX_GRADIENT: val = formations[i]->getVec(otherPose).length();
    00365                                   if(val > selectionValue)
    00366                                   {
    00367                                     selectionValue = val;
    00368                                     best = i;
    00369                                   }
    00370                                   break;
    00371         case SELECT_MIN_GRADIENT: val = formations[i]->getVec(otherPose).length();
    00372                                   if(val < selectionValue)
    00373                                   {
    00374                                     selectionValue = val;
    00375                                     best = i;
    00376                                   }
    00377                                   break;
    00378         case SELECT_MAX_PRIORITY: val = formations[i]->getPriority();
    00379                                   if(val > selectionValue)
    00380                                   {
    00381                                     selectionValue = val;
    00382                                     best = i;
    00383                                   }
    00384                                   break;
    00385         }
    00386       }
    00387     }
    00388     return best;
    00389   }
    00390   else
    00391   {
    00392     return -1;
    00393   }
    00394 }
    00395
    00396
    00397 SingleFormation* BestFitFormation::copy() const
    00398 {
    00399   BestFitFormation* bestFitFormation = new BestFitFormation();
    00400   bestFitFormation->bestFitSelection = bestFitSelection;
    00401   bestFitFormation->resultVecs.reserve(formations.size());
    00402   bestFitFormation->resultDistances.reserve(formations.size());
    00403   bestFitFormation->formations.reserve(formations.size());
    00404   std::vector< SingleFormation* >::const_iterator formation;
    00405   for(formation = formations.begin(); formation != formations.end(); ++formation)
    00406   {
    00407     SingleFormation* singleFormation = (*formation)->copy();
    00408     bestFitFormation->addFormation(singleFormation);
    00409   }
    00410   return bestFitFormation;
    00411 }
    00412
    00413
    00414 FormationObject::FormationObject(const std::string& name)
    00415 {
    00416   this->name = name;
    00417 }
    00418
    00419
    00420 FormationObject::~FormationObject()
    00421 {
    00422   std::vector < SingleFormation* >::iterator singleFormation;
    00423   for (singleFormation = singleFormations.begin ();
    00424        singleFormation != singleFormations.end (); ++singleFormation)
    00425   {
    00426     delete (*singleFormation);
    00427   }
    00428   singleFormations.clear();
    00429 }
    00430
    00431
    00432 PfPose FormationObject::getPose(const PfPose& robotPose)
    00433 {
    00434   PfVec bestPos;
    00435   double dist(DBL_MAX);
    00436   std::vector < SingleFormation* >::iterator formation;
    00437   for (formation = singleFormations.begin ();
    00438        formation != singleFormations.end (); ++formation)
    00439   {
    00440     PfVec pos((*formation)->getPosition(robotPose));
    00441     double d((pos-robotPose.pos).squareLength());
    00442     if(d < dist)
    00443     {
    00444       bestPos = pos;
    00445       dist = d;
    00446     }
    00447   }
    00448   PfPose bestPose;
    00449   bestPose.pos = bestPos;
    00450   return bestPose;
    00451 }
    00452
    00453
    00454 PfVec FormationObject::computeAbsFieldVecAt(const PfPose& otherPose)
    00455 {
    00456   PfVec formationVec;
    00457   std::vector < SingleFormation* >::const_iterator singleFormation;
    00458   for (singleFormation = singleFormations.begin ();
    00459        singleFormation != singleFormations.end (); ++singleFormation)
    00460   {
    00461     formationVec += (*singleFormation)->getVec(otherPose);
    00462   }
    00463   return formationVec;
    00464 }
    00465
    00466
    00467 double FormationObject::computeChargeAt(const PfPose& otherPose)
    00468 {
    00469   double charge(0.0);
    00470   std::vector < SingleFormation* >::const_iterator singleFormation;
    00471   for (singleFormation = singleFormations.begin ();
    00472        singleFormation != singleFormations.end (); ++singleFormation)
    00473   {
    00474     charge += (*singleFormation)->getCharge(otherPose);
    00475   }
    00476   return charge;
    00477 }
    00478
    00479
    00480 bool FormationObject::isStatic() const
    00481 {
    00482   std::vector < SingleFormation* >::const_iterator singleFormation;
    00483   for (singleFormation = singleFormations.begin ();
    00484        singleFormation != singleFormations.end (); ++singleFormation)
    00485   {
    00486     if(!(*singleFormation)->isStatic())
    00487     {
    00488       return false;
    00489     }
    00490   }
    00491   return true;
    00492 }
    00493
    00494
    00495 bool FormationObject::isActive() const
    00496 {
    00497   std::vector < SingleFormation* >::const_iterator singleFormation;
    00498   for (singleFormation = singleFormations.begin ();
    00499        singleFormation != singleFormations.end (); ++singleFormation)
    00500   {
    00501     if((*singleFormation)->isActive())
    00502     {
    00503       return true;
    00504     }
    00505   }
    00506   return false;
    00507 }
    00508
    00509
    00510 PotentialFieldsObject* FormationObject::getCopy()
    00511 {
    00512   FormationObject* newFormation = new FormationObject(name);
    00513   std::vector < SingleFormation* >::const_iterator singleFormation;
    00514   for (singleFormation = singleFormations.begin ();
    00515        singleFormation != singleFormations.end (); ++singleFormation)
    00516   {
    00517     SingleFormation* copyFormation = (*singleFormation)->copy();
    00518     newFormation->addSingleFormation(copyFormation);
    00519   }
    00520   return newFormation;
    00521 }