HelperFunctions.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the University of Toronto nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Authors: Jonathan Gammell */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_COMMONHELPERFUNCTIONS_
38 #define OMPL_GEOMETRIC_PLANNERS_BITSTAR_COMMONHELPERFUNCTIONS_
39 
40 // std::pair
41 #include <utility>
42 
44 // Anonymous helpers
45 namespace
46 {
48  template <typename T, typename U>
49  std::pair<T, U> operator+(const std::pair<T, U> &lhs, const std::pair<T, U> &rhs)
50  {
51  return std::make_pair(lhs.first + rhs.first, lhs.second + rhs.second);
52  };
53 
55  template <typename V>
56  void swapPopBack(typename V::iterator iter, V *vect)
57  {
58  // Swap to the back if not already there
59  if (iter != (vect->end() - 1))
60  {
61  std::swap(*iter, vect->back());
62  }
63 
64  // Delete the back of the vector
65  vect->pop_back();
66  };
67 
69  template <std::size_t SIZE>
70  std::ostream& operator<<(std::ostream& out, const std::array<ompl::base::Cost, SIZE>& costArray)
71  {
72  // Start with a bracket
73  out << "(";
74 
75  // Iterate through the values
76  for (unsigned int i = 0u; i < costArray.size(); ++i)
77  {
78  // Print value
79  out << costArray.at(i);
80 
81  // If not last, print a ,
82  if (i != costArray.size() - 1u)
83  {
84  out << ", ";
85  }
86  }
87 
88  // End with a bracket
89  out << ")";
90 
91  return out;
92  };
93 }
95 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_COMMONHELPERFUNCTIONS_
StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space from two existing state spaces. The components of this compound spac...