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

# Tools/PotentialFields/FieldObject.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file FieldObject.cpp
    00003 *
    00004 * Implementation of class PotentialFieldsObject
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009 #include "FieldObject.h"
    00010 #include "PotentialfieldComposition.h"
    00011
    00012 PotentialFieldsObject::PotentialFieldsObject()
    00013 {
    00014   refToPfieldComposition = 0;
    00015   geometry = 0;
    00016   absGeometry = 0;
    00017   function = 0;
    00018   tangentialField = NO_TANGENTIALFIELD;
    00019 }
    00020
    00021
    00022 PotentialFieldsObject::PotentialFieldsObject(const std::string& name, ObjectType objectType)
    00023 {
    00024   this->geometry = 0;
    00025   this->absGeometry = 0;
    00026   this->name = name;
    00027   this->objectType = objectType;
    00028   refToPfieldComposition = 0;
    00029   active = true;
    00030   function = 0;
    00031   tangentialField = NO_TANGENTIALFIELD;
    00032 }
    00033
    00034
    00035 PotentialFieldsObject::~PotentialFieldsObject()
    00036 {
    00037   if(geometry != 0)
    00038   {
    00039     delete geometry;
    00040   }
    00041   if(absGeometry != 0)
    00042   {
    00043     delete absGeometry;
    00044   }
    00045   if(function != 0)
    00046   {
    00047     delete function;
    00048   }
    00049 }
    00050
    00051
    00052 PotentialFieldsObject* PotentialFieldsObject::createInstance(const std::string& instanceName)
    00053 {
    00054   PotentialFieldsObject* instance = new PotentialFieldsObject();
    00055   (*instance) = (*this);
    00056   instance->name = instanceName;
    00057   return instance;
    00058 }
    00059
    00060
    00061 void PotentialFieldsObject::operator = (const PotentialFieldsObject& other)
    00062 {
    00063   name = other.name;
    00064   objectType = other.objectType;
    00065   active = other.active;
    00066   setPose(other.pose);
    00067   setActivation(other.active);
    00068   if(function != 0)
    00069   {
    00070     delete function;
    00071     function = 0;
    00072   }
    00073   setFunction(other.function);
    00074   if(geometry != 0)
    00075   {
    00076     delete geometry;
    00077     geometry = 0;
    00078   }
    00079   if(absGeometry != 0)
    00080   {
    00081     delete absGeometry;
    00082     absGeometry = 0;
    00083   }
    00084   setGeometry(other.geometry);
    00085   setTangentialField(other.tangentialField);
    00086   setStatic(other.isStaticVar, other.dynamicPoseId);
    00087   setPfieldCompositionRef(other.refToPfieldComposition);
    00088   switch(other.fieldType)
    00089   {
    00090     case SECTOR_FIELD: setField(other.fieldType, other.sector);
    00091     default: setField(other.fieldType);
    00092   }
    00093 }
    00094
    00095
    00096 PotentialFieldsObject* PotentialFieldsObject::getCopy()
    00097 {
    00098   PotentialFieldsObject* newObject = new PotentialFieldsObject();
    00099   (*newObject) = (*this);
    00100   return newObject;
    00101 }
    00102
    00103
    00104 void PotentialFieldsObject::getMinimalCopyFrom(PotentialFieldsObject* other)
    00105 {
    00106   if(other->isActive())
    00107   {
    00108     setPose(other->getPose());
    00109     setActivation(true);
    00110   }
    00111   else
    00112   {
    00113     setActivation(false);
    00114   }
    00115 }
    00116
    00117
    00118 void PotentialFieldsObject::setGeometry(PfieldGeometricObject* geometry)
    00119 {
    00120   if(geometry == 0)
    00121   {
    00122     this->geometry = 0;
    00123     absGeometry = 0;
    00124   }
    00125   else
    00126   {
    00127     this->geometry = geometry->clone();
    00128     absGeometry = geometry->getAbs(pose);
    00129   }
    00130 }
    00131
    00132
    00133 void PotentialFieldsObject::setPose(const PfPose& pose)
    00134 {
    00135   this->pose = pose;
    00136   if(absGeometry != 0)
    00137   {
    00138     absGeometry->setAbsoluteFromOther(pose, geometry);
    00139   }
    00140 }
    00141
    00142
    00143 double PotentialFieldsObject::computeChargeAt(const PfPose& otherPose)
    00144 {
    00145   double charge = 0.0;
    00146   if(active)
    00147   {
    00148     if(pose.hasProbabilityDistribution)
    00149     {
    00150       for(unsigned int j=0; j<pose.probabilityDistribution.size(); j++)
    00151       {
    00152         PfPose ownProbPose(pose.probabilityDistribution[j]);
    00153         if(otherPose.hasProbabilityDistribution)
    00154         {
    00155           for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
    00156           {
    00157             PfPose probPose(otherPose.probabilityDistribution[i]);
    00158             double probCharge(computeChargeForSinglePose(ownProbPose, probPose));
    00159             probCharge *= (probPose.probability * ownProbPose.probability);
    00160             charge += probCharge;
    00161           }
    00162         }
    00163         else
    00164         {
    00165           charge += ownProbPose.probability *
    00166                     computeChargeForSinglePose(ownProbPose, otherPose);
    00167         }
    00168       }
    00169     }
    00170     else
    00171     {
    00172       if(otherPose.hasProbabilityDistribution)
    00173       {
    00174         for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
    00175         {
    00176           PfPose probPose = otherPose.probabilityDistribution[i];
    00177           double probCharge(computeChargeForSinglePose(pose, probPose));
    00178           probCharge *= probPose.probability;
    00179           charge += probCharge;
    00180         }
    00181       }
    00182       else
    00183       {
    00184         charge = computeChargeForSinglePose(pose, otherPose);
    00185       }
    00186     }
    00187   }
    00188   return charge;
    00189 }
    00190
    00191
    00192 PfVec PotentialFieldsObject::computeAbsFieldVecAt(const PfPose& otherPose)
    00193 {
    00194   PfVec result(0.0,0.0);
    00195   if(active)
    00196   {
    00197     if(pose.hasProbabilityDistribution)
    00198     {
    00199       for(unsigned int j=0; j<pose.probabilityDistribution.size(); j++)
    00200       {
    00201         PfPose ownProbPose(pose.probabilityDistribution[j]);
    00202         if(otherPose.hasProbabilityDistribution)
    00203         {
    00204           for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
    00205           {
    00206             PfPose probPose(otherPose.probabilityDistribution[i]);
    00207             PfVec probVec(computeGradientVecForSinglePose(ownProbPose, probPose));
    00208             probVec *= (probPose.probability * ownProbPose.probability);
    00209             result += probVec;
    00210           }
    00211         }
    00212         else
    00213         {
    00214           result += (computeGradientVecForSinglePose(ownProbPose, otherPose) *
    00215                      ownProbPose.probability);
    00216         }
    00217       }
    00218     }
    00219     else
    00220     {
    00221       if(otherPose.hasProbabilityDistribution)
    00222       {
    00223         for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
    00224         {
    00225           PfPose probPose = otherPose.probabilityDistribution[i];
    00226           PfVec probVec(computeGradientVecForSinglePose(pose, probPose));
    00227           probVec *= otherPose.probabilityDistribution[i].probability;
    00228           result += probVec;
    00229         }
    00230       }
    00231       else
    00232       {
    00233         result = computeGradientVecForSinglePose(pose, otherPose);
    00234       }
    00235     }
    00236     if(tangentialField == CLOCKWISE)
    00237     {
    00238       if(objectType == REPULSIVE)
    00239       {
    00240         result.rotate(pi_2);
    00241       }
    00242       else
    00243       {
    00244         result.rotate(-pi_2);
    00245       }
    00246     }
    00247     else if(tangentialField == COUNTER_CLOCKWISE)
    00248     {
    00249       if(objectType == REPULSIVE)
    00250       {
    00251         result.rotate(-pi_2);
    00252       }
    00253       else
    00254       {
    00255         result.rotate(pi_2);
    00256       }
    00257     }
    00258   }
    00259   return result;
    00260 }
    00261
    00262
    00263 inline double PotentialFieldsObject::computeChargeForSinglePose(const PfPose& objectPose,
    00264                                                  const PfPose& position) const
    00265 {
    00266   double charge(0.0);
    00267   switch(fieldType)
    00268   {
    00269   case POINT_FIELD:  charge = computeChargeForPointfield(objectPose, position, function);
    00270                      break;
    00271   case SHAPE_FIELD:  charge = computeChargeForShapefield(objectPose, position, function, geometry);
    00272                      break;
    00273   case SECTOR_FIELD: charge = computeChargeForSectorfield(objectPose, position, function, sector);
    00274                      break;
    00275   }
    00276   return charge;
    00277 }
    00278
    00279
    00280 inline PfVec PotentialFieldsObject::computeGradientVecForSinglePose(const PfPose& objectPose,
    00281                                                      const PfPose& position) const
    00282 {
    00283   PfVec gradient;
    00284   switch(fieldType)
    00285   {
    00286   case POINT_FIELD:  gradient = computeGradientForPointfield(objectPose, position, function);
    00287     break;
    00288   case SHAPE_FIELD:  gradient = computeGradientForShapefield(objectPose, position, function,
    00289                        geometry, objectType);
    00290     break;
    00291   case SECTOR_FIELD: gradient = computeGradientForSectorfield(objectPose, position, function,
    00292                        sector);
    00293     break;
    00294   }
    00295   return gradient;
    00296 }
    00297
    00298
    00299 void PotentialFieldsObject::setFunction(PotentialfieldFunction* function)
    00300 {
    00301   if(this->function != 0)
    00302   {
    00303     delete (this->function);
    00304     this->function = 0;
    00305   }
    00306   if(function != 0)
    00307   {
    00308     this->function = function->clone();
    00309   }
    00310 }
    00311
    00312
    00313 void PotentialFieldsObject::setField(FieldType type)
    00314 {
    00315   fieldType = type;
    00316 }
    00317
    00318
    00319 void PotentialFieldsObject::setField(FieldType type, const Sector& sector)
    00320 {
    00321   fieldType = type;
    00322   this->sector = sector;
    00323   this->sector.crossFunction = sector.crossFunction->clone();
    00324 }
    00325
    00326
    00327 void PotentialFieldsObject::updateData()
    00328 {
    00329   ObjectStateDescription desc;
    00330   desc = refToPfieldComposition->getDescriptionFromId(dynamicPoseId);
    00331   setPose(desc.pose);
    00332   active = desc.isActive;
    00333 }
    00334
    00335
    00336 void PotentialFieldsObject::computeAbsGeometry()
    00337 {
    00338   absGeometry->setAbsoluteFromOther(pose, geometry);
    00339 }
    00340