Tools/PotentialFields/FormationObject.cpp

Go to the documentation of this file.

/**
* @file FormationObject.cpp
* 
* Implementation of class FormationObject
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/


#include <cassert>
#include <cfloat>
#include "FormationObject.h"
#include "PfieldGeometry.h"



SingleFormation::SingleFormation()
{
  geometricFormationObject.setField(SHAPE_FIELD);
}


bool SingleFormation::isStatic() const
{
  std::vector < Object* >::const_iterator object;
  for (object = objects.begin (); object != objects.end (); ++object)
  {
    if(!(*object)->isStatic())
    {
      return false;
    }
  }
  return true;
}


PfVec SingleFormation::getPosition(const PfPose& robotPose)
{
  robotPosition = robotPose.pos;
  updateGeometry();
  Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
  PfVec nextPos;
  double dist(polygon->distanceTo(PfPose(), robotPose.pos, nextPos));
  if(dist != 0.0)
  {
    return nextPos;
  }
  else
  {
    return PfVec(0.0,0.0);
  }
}


PfVec SingleFormation::getVec(const PfPose& otherPose)
{
  robotPosition = otherPose.pos;
  updateGeometry();
  if(positionInsideFormation(otherPose.pos))
  {
    return PfVec(0.0,0.0);
  }
  else
  {
    return geometricFormationObject.computeAbsFieldVecAt(otherPose);
  }
}


double SingleFormation::getCharge(const PfPose& otherPose)
{
  robotPosition = otherPose.pos;
  updateGeometry();
  return geometricFormationObject.computeChargeAt(otherPose);
}


void SingleFormation::addObject(Object* object)
{
  objects.push_back(object);
}


double SingleFormation::getDistanceToFormation(const PfPose& otherPose) const
{
  PfieldGeometricObject* obj = geometricFormationObject.getGeometry();
  PfPose dummyPose; PfVec dummyVec;
  return obj->distanceTo(dummyPose, otherPose.pos, dummyVec);
}


SingleFormation* SingleFormation::copy() const
{
  SingleFormation* singleFormation = new SingleFormation();
  singleFormation->geometricFormationObject = geometricFormationObject;
  singleFormation->objects = objects;
  singleFormation->robotPosition = robotPosition;
  singleFormation->priority = priority;
  return singleFormation;
}


BetweenFormation::BetweenFormation()
{
  Polygon* polygon = new Polygon();
  geometricFormationObject.setGeometry(polygon);
  delete polygon;
}


bool BetweenFormation::isActive() const
{
  return (objects[0]->isActive() && objects[1]->isActive());
}


bool BetweenFormation::positionInsideFormation(const PfVec& p) const
{
  Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
  return (polygon->pointInside(p));
}


void BetweenFormation::updateGeometry()
{
  Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
  polygon->pts.clear();
  std::vector < Object* >::const_iterator object;
  for (object = objects.begin (); object != objects.end (); ++object)
  {
    (*object)->getAbsGeometry()->getPoints(polygon->pts);
  }
  if(polygon->pts.size() > 3)
  {
    reduceToConvexHullByWrapping(*polygon);
  }
  polygon->initRadiusOfCollisionCircle();
}


AmongFormation::AmongFormation()
{
  Polygon* polygon = new Polygon();
  geometricFormationObject.setGeometry(polygon);
  delete polygon;
}


bool AmongFormation::isActive() const
{
  int numberOfActiveObjects(0);
  std::vector < Object* >::const_iterator object;
  for (object = objects.begin(); object != objects.end(); ++object)
  {
    if((*object)->isActive())
    {
      ++numberOfActiveObjects;
    }
    if(numberOfActiveObjects >= 2)
    {
      return true;
    }
  }  
  return false;
}


void AmongFormation::updateGeometry()
{
  Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
  polygon->pts.clear();
  std::vector < Object* >::const_iterator object;
  for (object = objects.begin(); object != objects.end(); ++object)
  {
    (*object)->getAbsGeometry()->getPoints(polygon->pts);
  }
  if(polygon->pts.size() > 3)
  {
    reduceToConvexHullByWrapping(*polygon);
  }
  polygon->initRadiusOfCollisionCircle();
}


bool AmongFormation::positionInsideFormation(const PfVec& p) const
{
  Polygon* polygon = (Polygon*)geometricFormationObject.getGeometry();
  return (polygon->pointInside(p));
}


RelativeFormation::RelativeFormation()
{
  Line* line = new Line();
  geometricFormationObject.setGeometry(line);
  delete line;
}


bool RelativeFormation::isActive() const
{
  return (objects[0]->isActive());
}


void RelativeFormation::updateGeometry()
{
  Line* line = (Line*)geometricFormationObject.getGeometry();
  PfPose objectPose(objects[0]->getPose());
  line->p1 = objectPose.pos;
  PfVec vecToP2(1.0,0.0);
  vecToP2.rotate(angle);
  if(coordinates == RELATIVE_FORMATION)
  {
    vecToP2.rotate(objectPose.rotation);
  }
  vecToP2 *= (line->p1.distanceTo(robotPosition)*2);
  line->p2 = line->p1 + vecToP2;
}


SingleFormation* RelativeFormation::copy() const
{
  RelativeFormation* relativeFormation = new RelativeFormation();
  relativeFormation->geometricFormationObject = geometricFormationObject;
  relativeFormation->objects = objects;
  relativeFormation->robotPosition = robotPosition;
  relativeFormation->angle = angle;
  relativeFormation->coordinates = coordinates;
  return relativeFormation;
}


BestFitFormation::BestFitFormation()
{
}


BestFitFormation::~BestFitFormation()
{
  std::vector < SingleFormation* >::iterator formation;
  for (formation = formations.begin (); 
       formation != formations.end (); ++formation)
  {
    delete (*formation);
  }
}


bool BestFitFormation::isStatic() const
{
  std::vector < SingleFormation* >::const_iterator formation;
  for (formation = formations.begin (); 
       formation != formations.end (); ++formation)
  {
    if(!(*formation)->isStatic())
    {
      return false;
    }
  }
  return true;
}


PfVec BestFitFormation::getPosition(const PfPose& robotPose) 
{
  int best(findBestFormation(robotPose));
  if(best == -1)
  {
    return PfVec(0.0,0.0);
  }
  else
  {
    return formations[(unsigned int)best]->getPosition(robotPose);
  }
}


PfVec BestFitFormation::getVec(const PfPose& otherPose)
{
  int best(findBestFormation(otherPose));
  if(best == -1)
  {
    return PfVec(0.0,0.0);
  }
  else
  {
    return formations[(unsigned int)best]->getVec(otherPose);;
  }
}


double BestFitFormation::getCharge(const PfPose& otherPose)
{
  int best(findBestFormation(otherPose));
  if(best == -1)
  {
    return 0.0;
  }
  else
  {
    return formations[(unsigned int)best]->getCharge(otherPose);
  }
}


bool BestFitFormation::isActive() const 
{
  std::vector < SingleFormation* >::const_iterator formation;
  for (formation = formations.begin (); 
       formation != formations.end (); ++formation)
  {
    if((*formation)->isActive())
    {
      return true;
    }
  }
  return false;
}


int BestFitFormation::findBestFormation(const PfPose& otherPose)
{
  unsigned int best(0);
  while(best < formations.size())
  {
    if(formations[best]->isActive())
    {
      break;
    }
    else
    {
      best++;
    }
  }
  if(best < formations.size())
  {
    double selectionValue(0.0);
    switch(bestFitSelection)
    {
    case SELECT_MIN_DISTANCE: selectionValue = formations[best]->getDistanceToFormation(otherPose);
                              break;
    case SELECT_MAX_GRADIENT: selectionValue = formations[best]->getVec(otherPose).length();
                              break;
    case SELECT_MIN_GRADIENT: selectionValue = formations[best]->getVec(otherPose).length();
                              break;
    case SELECT_MAX_PRIORITY: selectionValue = formations[best]->getPriority();
                              break;
    }
    for(unsigned int i = best+1; i < formations.size(); i++)
    {
      if(formations[i]->isActive())
      {
        double val;
        switch(bestFitSelection)
        {
        case SELECT_MIN_DISTANCE: val = formations[i]->getDistanceToFormation(otherPose);
                                  if(val < selectionValue)
                                  {
                                    selectionValue = val;
                                    best = i;
                                  }
                                  break;
        case SELECT_MAX_GRADIENT: val = formations[i]->getVec(otherPose).length();
                                  if(val > selectionValue)
                                  {
                                    selectionValue = val;
                                    best = i;
                                  }
                                  break;
        case SELECT_MIN_GRADIENT: val = formations[i]->getVec(otherPose).length();
                                  if(val < selectionValue)
                                  {
                                    selectionValue = val;
                                    best = i;
                                  }
                                  break;
        case SELECT_MAX_PRIORITY: val = formations[i]->getPriority();
                                  if(val > selectionValue)
                                  {
                                    selectionValue = val;
                                    best = i;
                                  }
                                  break;
        }
      }
    }
    return best;
  }
  else
  {
    return -1;
  }
}


SingleFormation* BestFitFormation::copy() const
{
  BestFitFormation* bestFitFormation = new BestFitFormation();
  bestFitFormation->bestFitSelection = bestFitSelection;
  bestFitFormation->resultVecs.reserve(formations.size());
  bestFitFormation->resultDistances.reserve(formations.size());
  bestFitFormation->formations.reserve(formations.size());
  std::vector< SingleFormation* >::const_iterator formation;
  for(formation = formations.begin(); formation != formations.end(); ++formation)
  {
    SingleFormation* singleFormation = (*formation)->copy();
    bestFitFormation->addFormation(singleFormation);
  }
  return bestFitFormation;
}


FormationObject::FormationObject(const std::string& name)
{
  this->name = name;
}


FormationObject::~FormationObject()
{
  std::vector < SingleFormation* >::iterator singleFormation;
  for (singleFormation = singleFormations.begin (); 
       singleFormation != singleFormations.end (); ++singleFormation)
  {
    delete (*singleFormation);
  }
  singleFormations.clear();
}


PfPose FormationObject::getPose(const PfPose& robotPose) 
{
  PfVec bestPos;
  double dist(DBL_MAX);
  std::vector < SingleFormation* >::iterator formation;
  for (formation = singleFormations.begin (); 
       formation != singleFormations.end (); ++formation)
  {
    PfVec pos((*formation)->getPosition(robotPose));
    double d((pos-robotPose.pos).squareLength());
    if(d < dist)
    {
      bestPos = pos;
      dist = d;
    }
  }
  PfPose bestPose;
  bestPose.pos = bestPos;
  return bestPose;
}


PfVec FormationObject::computeAbsFieldVecAt(const PfPose& otherPose) 
{
  PfVec formationVec;
  std::vector < SingleFormation* >::const_iterator singleFormation;
  for (singleFormation = singleFormations.begin (); 
       singleFormation != singleFormations.end (); ++singleFormation)
  {
    formationVec += (*singleFormation)->getVec(otherPose);
  }
  return formationVec;
}
  

double FormationObject::computeChargeAt(const PfPose& otherPose)
{
  double charge(0.0);
  std::vector < SingleFormation* >::const_iterator singleFormation;
  for (singleFormation = singleFormations.begin (); 
       singleFormation != singleFormations.end (); ++singleFormation)
  {
    charge += (*singleFormation)->getCharge(otherPose);
  }
  return charge;
}


bool FormationObject::isStatic() const
{
  std::vector < SingleFormation* >::const_iterator singleFormation;
  for (singleFormation = singleFormations.begin (); 
       singleFormation != singleFormations.end (); ++singleFormation)
  {
    if(!(*singleFormation)->isStatic())
    {
      return false;
    }
  }
  return true;
}


bool FormationObject::isActive() const
{
  std::vector < SingleFormation* >::const_iterator singleFormation;
  for (singleFormation = singleFormations.begin (); 
       singleFormation != singleFormations.end (); ++singleFormation)
  {
    if((*singleFormation)->isActive())
    {
      return true;
    }
  }
  return false;
}


Object* FormationObject::getCopy()
{
  FormationObject* newFormation = new FormationObject(name);
  std::vector < SingleFormation* >::const_iterator singleFormation;
  for (singleFormation = singleFormations.begin (); 
       singleFormation != singleFormations.end (); ++singleFormation)
  {
    SingleFormation* copyFormation = (*singleFormation)->copy();
    newFormation->addSingleFormation(copyFormation);
  }
  return newFormation;
}



/*
* $Log: FormationObject.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:37:26  cvsadm
* created new repository GT2004_WM
*
* Revision 1.1  2004/01/20 15:42:19  tim
* Added potential fields implementation
*
*/

---