All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
PlannerData.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/base/PlannerData.h"
00038 
00039 void ompl::base::PlannerData::clear(void)
00040 {
00041     stateIndex.clear();
00042     states.clear();
00043     tags.clear();
00044     edges.clear();
00045     properties.clear();
00046     si.reset();
00047 }
00048 
00049 void ompl::base::PlannerData::tagState(const State *s, int tag)
00050 {
00051     if (s != NULL)
00052     {
00053         std::map<const State*, unsigned int>::iterator it = stateIndex.find(s);
00054         if (it == stateIndex.end())
00055         {
00056             unsigned int p = states.size();
00057             states.push_back(s);
00058             tags.push_back(tag);
00059             stateIndex[s] = p;
00060             edges.resize(states.size());
00061         }
00062         else
00063             tags[it->second] = tag;
00064     }
00065 }
00066 
00067 int ompl::base::PlannerData::recordEdge(const State *s1, const State *s2)
00068 {
00069     if (s1 == NULL || s2 == NULL)
00070     {
00071         const State *s = s1 == NULL ? s2 : s1;
00072         if (s != NULL)
00073         {
00074             std::map<const State*, unsigned int>::iterator it = stateIndex.find(s);
00075             if (it == stateIndex.end())
00076             {
00077                 unsigned int p = states.size();
00078                 states.push_back(s);
00079                 tags.push_back(0);
00080                 stateIndex[s] = p;
00081                 edges.resize(states.size());
00082             }
00083         }
00084         return -1;
00085     }
00086     else
00087     {
00088         std::map<const State*, unsigned int>::iterator it1 = stateIndex.find(s1);
00089         std::map<const State*, unsigned int>::iterator it2 = stateIndex.find(s2);
00090 
00091         bool newEdge = false;
00092 
00093         unsigned int p1;
00094         if (it1 == stateIndex.end())
00095         {
00096             p1 = states.size();
00097             states.push_back(s1);
00098             tags.push_back(0);
00099             stateIndex[s1] = p1;
00100             edges.resize(states.size());
00101             newEdge = true;
00102         }
00103         else
00104             p1 = it1->second;
00105 
00106         unsigned int p2;
00107         if (it2 == stateIndex.end())
00108         {
00109             p2 = states.size();
00110             states.push_back(s2);
00111             tags.push_back(0);
00112             stateIndex[s2] = p2;
00113             edges.resize(states.size());
00114             newEdge = true;
00115         }
00116         else
00117             p2 = it2->second;
00118 
00119         // if we are not yet sure this is a new edge, we check indeed if this edge exists
00120         if (!newEdge)
00121         {
00122             newEdge = true;
00123             for (unsigned int i = 0 ; i < edges[p1].size() ; ++i)
00124                 if (edges[p1][i] == p2)
00125                 {
00126                     newEdge = false;
00127                     break;
00128                 }
00129         }
00130 
00131         if (newEdge)
00132         {
00133             edges[p1].push_back(p2);
00134             return p1;
00135         }
00136         else
00137             return -1;
00138     }
00139 }
00140 
00141 void ompl::base::PlannerData::print(std::ostream &out) const
00142 {
00143     out << states.size() << std::endl;
00144     for (unsigned int i = 0 ; i < states.size() ; ++i)
00145     {
00146         out << i << " (tag="<< tags[i] << "): ";
00147         if (si)
00148             si->printState(states[i], out);
00149         else
00150             out << states[i] << std::endl;
00151     }
00152 
00153     for (unsigned int i = 0 ; i < edges.size() ; ++i)
00154     {
00155         if (edges[i].empty())
00156             continue;
00157         out << i << ": ";
00158         for (unsigned int j = 0 ; j < edges[i].size() ; ++j)
00159             out << edges[i][j] << ' ';
00160         out << std::endl;
00161     }
00162 }