Tools/PotentialFields/AStarSearch.cpp

Go to the documentation of this file.

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


#include "AStarSearch.h"
#include "Pfield.h"


void PotentialfieldAStarNode::setValueAtPos(
              const PotentialfieldAStarParameterSet& parameterSet)
{ 
  valueAtPos = parameterSet.field->getFieldValueAt(pose);
  if(parameterSet.useStabilization && parameterSet.numberOfCalls)
  {
    valueAtPos += parameterSet.stabilizationObject->computeChargeAt(pose);
  }
}


void PotentialfieldAStarNode::expand(std::vector<PotentialfieldAStarNode>& searchTree,
              const std::vector<unsigned int>& expandedNodes,
              const PotentialfieldAStarNode& goal,
              const PotentialfieldAStarParameterSet& parameterSet,
              unsigned int ownNodeNum)
{
  expanded = true;
  PotentialfieldAStarNode newNode;
  unsigned int currentBranchingFactor;
  computeCurrentParameters(expansionRadius, currentBranchingFactor, parameterSet);
  PfPose newPose;
  double newG, newH;
  double angleBetweenNodes(pi2 / (double)currentBranchingFactor);
  PfVec nodePos(expansionRadius,0.0);
  PfPose goalPose(goal.getPose());
  PfVec parentPos(searchTree[parentNode].getPose().pos);
  double angleToParent(pose.getAngleTo(parentPos));
  for(unsigned int i=0; i<currentBranchingFactor; i++)
  {
    newPose = pose;
    newPose.addVector(nodePos);
    if(((parentNode==0) || (fabs(pose.getAngleTo(newPose.pos) - angleToParent) >= (pi/3)))
        && (!tooCloseToAnotherNode(searchTree, expandedNodes, newPose)))
    {
      newNode.setPose(newPose);
      newNode.setValueAtPos(parameterSet);
      newNode.setParentNode(ownNodeNum);
      newNode.expansionRadius = expansionRadius;
      newG = gValue + computeCostsTo(newNode, parameterSet);
      newH = computeHeuristicBetween(newPose, goalPose, currentBranchingFactor);
      newNode.setFunctionValues(newG, newH);
      searchTree.push_back(newNode);
    }
    nodePos.rotate(angleBetweenNodes);
  }
}


bool PotentialfieldAStarNode::hasReached(
                                 const PotentialfieldAStarNode& goal,
                                 const PotentialfieldAStarParameterSet& parameterSet) const
{
  double distanceToGoal((goal.getPose().pos-pose.pos).length());
  return ((distanceToGoal <= parameterSet.distanceToGoal) || 
          (distanceToGoal <= expansionRadius));
}


inline double PotentialfieldAStarNode::computeCostsTo(const PotentialfieldAStarNode& node,
                                                      const PotentialfieldAStarParameterSet& parameterSet) const
{
  double valDiff(node.valueAtPos - this->valueAtPos);
  if(valDiff <= 0.0)
  {
    return expansionRadius;
  }
  else
  {
    return expansionRadius + valDiff;
  }
}


inline double PotentialfieldAStarNode::computeHeuristicBetween(
                  const PfPose& p1, const PfPose& p2,
                  unsigned int currentBranchingFactor) const
{
  double angleToP2(p1.getAngleTo(p2.pos));
  double angleBetween(pi2 / (double)currentBranchingFactor);
  double angleRatio(angleToP2 / angleBetween);
  if((angleRatio - floor(angleRatio)) < EPSILON)
  {
    double distance((p2.pos-p1.pos).length());
    return floor(distance/expansionRadius)*expansionRadius;
  }
  else
  {
    double smallerAngle(0.0), largerAngle(0.0);
    if(angleToP2 > 0)
    {
      while(largerAngle < angleToP2)
      {
        largerAngle += angleBetween;
      }
      smallerAngle = largerAngle - angleBetween;
    }
    else
    {
      while(smallerAngle > angleToP2)
      {
        smallerAngle -= angleBetween;
      }
      largerAngle = smallerAngle + angleBetween;
    }
    PfVec vs(1.0,0.0);
    PfVec vl(1.0,0.0);
    vs.rotate(smallerAngle);
    vl.rotate(largerAngle);
    PfVec pDiff(p1.pos-p2.pos);
    double n((pDiff.x*vl.y - pDiff.y*vl.x) / (vs.x*vl.y - vs.y*vl.x));
    PfVec intersectionPos(vs);
    intersectionPos *= n;
    intersectionPos += p2.pos;
    double dist1((p1.pos-intersectionPos).length());
    double dist2((p2.pos-intersectionPos).length());
    return (floor(dist1/expansionRadius)*expansionRadius +
            floor(dist2/expansionRadius)*expansionRadius);
  }
}


inline void PotentialfieldAStarNode::computeCurrentParameters(
                                     double& currentExpansionRadius, 
                                     unsigned int& currentBranchingFactor,
                                     const PotentialfieldAStarParameterSet& parameterSet) const
{
  double dist(pose.pos.distanceTo(parameterSet.startPosition));
  if(dist < parameterSet.endOfNear)
  {
    currentExpansionRadius = parameterSet.minExpansionRadius;
    currentBranchingFactor = parameterSet.maxBranchingFactor;
  }
  else if(dist > parameterSet.endOfFar)
  {
    currentExpansionRadius = parameterSet.maxExpansionRadius;
    currentBranchingFactor = parameterSet.minBranchingFactor;
  }
  else
  {
    double distPercentage((dist-parameterSet.endOfNear)/(parameterSet.endOfFar - parameterSet.endOfNear));
    currentExpansionRadius = parameterSet.minExpansionRadius + distPercentage*
                    (parameterSet.maxExpansionRadius - parameterSet.minExpansionRadius);
    currentBranchingFactor = parameterSet.maxBranchingFactor - (unsigned int)(floor(distPercentage*
                    (double)(parameterSet.maxBranchingFactor - parameterSet.minBranchingFactor)));
  }
}


bool PotentialfieldAStarNode::tooCloseToAnotherNode(
                             std::vector<PotentialfieldAStarNode>& searchTree,
                             const std::vector<unsigned int>& expandedNodes,
                             const PfPose& pose) const
{
  for(unsigned int i=0; i<expandedNodes.size(); i++)
  {
    if(searchTree[expandedNodes[i]].getPose().pos.distanceTo(pose.pos) < 
       searchTree[expandedNodes[i]].getExpansionRadius()*0.95)
    {
      return true;
    }
  }
  return false;
}



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

---