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 void 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     }
00085     else
00086     {
00087         std::map<const State*, unsigned int>::iterator it1 = stateIndex.find(s1);
00088         std::map<const State*, unsigned int>::iterator it2 = stateIndex.find(s2);
00089 
00090         bool newEdge = false;
00091 
00092         unsigned int p1;
00093         if (it1 == stateIndex.end())
00094         {
00095             p1 = states.size();
00096             states.push_back(s1);
00097             tags.push_back(0);
00098             stateIndex[s1] = p1;
00099             edges.resize(states.size());
00100             newEdge = true;
00101         }
00102         else
00103             p1 = it1->second;
00104 
00105         unsigned int p2;
00106         if (it2 == stateIndex.end())
00107         {
00108             p2 = states.size();
00109             states.push_back(s2);
00110             tags.push_back(0);
00111             stateIndex[s2] = p2;
00112             edges.resize(states.size());
00113             newEdge = true;
00114         }
00115         else
00116             p2 = it2->second;
00117 
00118         // if we are not yet sure this is a new edge, we check indeed if this edge exists
00119         if (!newEdge)
00120         {
00121             newEdge = true;
00122             for (unsigned int i = 0 ; i < edges[p1].size() ; ++i)
00123                 if (edges[p1][i] == p2)
00124                 {
00125                     newEdge = false;
00126                     break;
00127                 }
00128         }
00129 
00130         if (newEdge)
00131             edges[p1].push_back(p2);
00132     }
00133 }
00134 
00135 void ompl::base::PlannerData::print(std::ostream &out) const
00136 {
00137     out << states.size() << std::endl;
00138     for (unsigned int i = 0 ; i < states.size() ; ++i)
00139     {
00140         out << i << " (tag="<< tags[i] << "): ";
00141         if (si)
00142             si->printState(states[i], out);
00143         else
00144             out << states[i] << std::endl;
00145     }
00146 
00147     for (unsigned int i = 0 ; i < edges.size() ; ++i)
00148     {
00149         if (edges[i].empty())
00150             continue;
00151         out << i << ": ";
00152         for (unsigned int j = 0 ; j < edges[i].size() ; ++j)
00153             out << edges[i][j] << ' ';
00154         out << std::endl;
00155     }
00156 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator