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

# Tools/PotentialFields/AStarSearch.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file AStarSearch.cpp
    00003 *
    00004 * Implementation of class AStarNode
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009
    00010 #include "AStarSearch.h"
    00011 #include "Pfield.h"
    00012
    00013
    00014 void PotentialfieldAStarNode::setValueAtPos(
    00015               const PotentialfieldAStarParameterSet& parameterSet)
    00016 {
    00017   valueAtPos = parameterSet.field->getFieldValueAt(pose);
    00018   if(parameterSet.useStabilization && parameterSet.numberOfCalls)
    00019   {
    00020     valueAtPos += parameterSet.stabilizationObject->computeChargeAt(pose);
    00021   }
    00022 }
    00023
    00024
    00025 void PotentialfieldAStarNode::expand(std::vector<PotentialfieldAStarNode>& searchTree,
    00026               const std::vector<unsigned int>& expandedNodes,
    00027               const PotentialfieldAStarNode& goal,
    00028               const PotentialfieldAStarParameterSet& parameterSet,
    00029               unsigned int ownNodeNum)
    00030 {
    00031   expanded = true;
    00032   PotentialfieldAStarNode newNode;
    00033   unsigned int currentBranchingFactor;
    00034   computeCurrentParameters(expansionRadius, currentBranchingFactor, parameterSet);
    00035   PfPose newPose;
    00036   double newG, newH;
    00037   double angleBetweenNodes(pi2 / (double)currentBranchingFactor);
    00038   PfVec nodePos(expansionRadius,0.0);
    00039   PfPose goalPose(goal.getPose());
    00040   PfVec parentPos(searchTree[parentNode].getPose().pos);
    00041   double angleToParent(pose.getAngleTo(parentPos));
    00042   for(unsigned int i=0; i<currentBranchingFactor; i++)
    00043   {
    00044     newPose = pose;
    00045     newPose.addVector(nodePos);
    00046     if(((parentNode==0) || (fabs(pose.getAngleTo(newPose.pos) - angleToParent) >= (pi/3)))
    00047         && (!tooCloseToAnotherNode(searchTree, expandedNodes, newPose)))
    00048     {
    00049       newNode.setPose(newPose);
    00050       newNode.setValueAtPos(parameterSet);
    00051       newNode.setParentNode(ownNodeNum);
    00052       newNode.expansionRadius = expansionRadius;
    00053       newG = gValue + computeCostsTo(newNode, parameterSet);
    00054       newH = computeHeuristicBetween(newPose, goalPose, currentBranchingFactor);
    00055       newNode.setFunctionValues(newG, newH);
    00056       searchTree.push_back(newNode);
    00057     }
    00058     nodePos.rotate(angleBetweenNodes);
    00059   }
    00060 }
    00061
    00062
    00063 bool PotentialfieldAStarNode::hasReached(
    00064                                  const PotentialfieldAStarNode& goal,
    00065                                  const PotentialfieldAStarParameterSet& parameterSet) const
    00066 {
    00067   double distanceToGoal((goal.getPose().pos-pose.pos).length());
    00068   return ((distanceToGoal <= parameterSet.distanceToGoal) ||
    00069           (distanceToGoal <= expansionRadius));
    00070 }
    00071
    00072
    00073 inline double PotentialfieldAStarNode::computeCostsTo(const PotentialfieldAStarNode& node,
    00074                                                       const PotentialfieldAStarParameterSet& parameterSet) const
    00075 {
    00076   double valDiff(node.valueAtPos - this->valueAtPos);
    00077   if(valDiff <= 0.0)
    00078   {
    00079     return expansionRadius;
    00080   }
    00081   else
    00082   {
    00083     return expansionRadius + valDiff;
    00084   }
    00085 }
    00086
    00087
    00088 inline double PotentialfieldAStarNode::computeHeuristicBetween(
    00089                   const PfPose& p1, const PfPose& p2,
    00090                   unsigned int currentBranchingFactor) const
    00091 {
    00092   double angleToP2(p1.getAngleTo(p2.pos));
    00093   double angleBetween(pi2 / (double)currentBranchingFactor);
    00094   double angleRatio(angleToP2 / angleBetween);
    00095   if((angleRatio - floor(angleRatio)) < EPSILON)
    00096   {
    00097     double distance((p2.pos-p1.pos).length());
    00098     return floor(distance/expansionRadius)*expansionRadius;
    00099   }
    00100   else
    00101   {
    00102     double smallerAngle(0.0), largerAngle(0.0);
    00103     if(angleToP2 > 0)
    00104     {
    00105       while(largerAngle < angleToP2)
    00106       {
    00107         largerAngle += angleBetween;
    00108       }
    00109       smallerAngle = largerAngle - angleBetween;
    00110     }
    00111     else
    00112     {
    00113       while(smallerAngle > angleToP2)
    00114       {
    00115         smallerAngle -= angleBetween;
    00116       }
    00117       largerAngle = smallerAngle + angleBetween;
    00118     }
    00119     PfVec vs(1.0,0.0);
    00120     PfVec vl(1.0,0.0);
    00121     vs.rotate(smallerAngle);
    00122     vl.rotate(largerAngle);
    00123     PfVec pDiff(p1.pos-p2.pos);
    00124     double n((pDiff.x*vl.y - pDiff.y*vl.x) / (vs.x*vl.y - vs.y*vl.x));
    00125     PfVec intersectionPos(vs);
    00126     intersectionPos *= n;
    00127     intersectionPos += p2.pos;
    00128     double dist1((p1.pos-intersectionPos).length());
    00129     double dist2((p2.pos-intersectionPos).length());
    00130     return (floor(dist1/expansionRadius)*expansionRadius +
    00131             floor(dist2/expansionRadius)*expansionRadius);
    00132   }
    00133 }
    00134
    00135
    00136 inline void PotentialfieldAStarNode::computeCurrentParameters(
    00137                                      double& currentExpansionRadius,
    00138                                      unsigned int& currentBranchingFactor,
    00139                                      const PotentialfieldAStarParameterSet& parameterSet) const
    00140 {
    00141   double dist(pose.pos.distanceTo(parameterSet.startPosition));
    00142   if(dist < parameterSet.endOfNear)
    00143   {
    00144     currentExpansionRadius = parameterSet.minExpansionRadius;
    00145     currentBranchingFactor = parameterSet.maxBranchingFactor;
    00146   }
    00147   else if(dist > parameterSet.endOfFar)
    00148   {
    00149     currentExpansionRadius = parameterSet.maxExpansionRadius;
    00150     currentBranchingFactor = parameterSet.minBranchingFactor;
    00151   }
    00152   else
    00153   {
    00154     double distPercentage((dist-parameterSet.endOfNear)/(parameterSet.endOfFar - parameterSet.endOfNear));
    00155     currentExpansionRadius = parameterSet.minExpansionRadius + distPercentage*
    00156                     (parameterSet.maxExpansionRadius - parameterSet.minExpansionRadius);
    00157     currentBranchingFactor = parameterSet.maxBranchingFactor - (unsigned int)(floor(distPercentage*
    00158                     (double)(parameterSet.maxBranchingFactor - parameterSet.minBranchingFactor)));
    00159   }
    00160 }
    00161
    00162
    00163 bool PotentialfieldAStarNode::tooCloseToAnotherNode(
    00164                              std::vector<PotentialfieldAStarNode>& searchTree,
    00165                              const std::vector<unsigned int>& expandedNodes,
    00166                              const PfPose& pose) const
    00167 {
    00168   for(unsigned int i=0; i<expandedNodes.size(); i++)
    00169   {
    00170     if(searchTree[expandedNodes[i]].getPose().pos.distanceTo(pose.pos) <
    00171        searchTree[expandedNodes[i]].getExpansionRadius()*0.95)
    00172     {
    00173       return true;
    00174     }
    00175   }
    00176   return false;
    00177 }