DART  6.7.3
UnorderedPairs.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
34 #define DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
35 
36 #include <unordered_map>
37 #include <unordered_set>
38 
39 namespace dart {
40 namespace collision {
41 namespace detail {
42 
43 template <class T>
45 {
46 public:
48  void addPair(const T* left, const T* right);
49 
51  void removePair(const T* left, const T* right);
52 
55 
57  bool contains(const T* left, const T* right) const;
58 
59 private:
65  std::unordered_map<const T*, std::unordered_set<const T*>> mList;
66 };
67 
68 //==============================================================================
69 template <class T>
70 void UnorderedPairs<T>::addPair(const T* left, const T* right)
71 {
72  if (!left || !right)
73  return;
74 
75  const auto* less = left;
76  const auto* greater = right;
77 
78  if (less > greater)
79  std::swap(less, greater);
80 
81  // Call insert in case an entry for bodyNodeLess doesn't exist. If it doesn't
82  // exist, it will be initialized with an empty set. If it does already exist,
83  // we will just get an iterator to the existing entry.
84  const auto itLess = mList.insert(
85  std::make_pair(less, std::unordered_set<const T*>())).first;
86 
87  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
88  // pair already existed, this will do nothing.
89  itLess->second.insert(greater);
90 }
91 
92 //==============================================================================
93 template<class T>
94 void UnorderedPairs<T>::removePair(const T* left, const T* right)
95 {
96  if (!left || !right)
97  return;
98 
99  const auto* bodyNodeLess = left;
100  const auto* bodyNodeGreater = right;
101 
102  if (bodyNodeLess > bodyNodeGreater)
103  std::swap(bodyNodeLess, bodyNodeGreater);
104 
105  // Remove the pair only when it already exists
106  const auto resultLeft = mList.find(bodyNodeLess);
107  const bool foundLeft = (resultLeft != mList.end());
108  if (foundLeft)
109  {
110  auto& associatedRights = resultLeft->second;
111  associatedRights.erase(bodyNodeGreater);
112 
113  if (associatedRights.empty())
114  mList.erase(resultLeft);
115  }
116 }
117 
118 //==============================================================================
119 template<class T>
121 {
122  mList.clear();
123 }
124 
125 //==============================================================================
126 template<class T>
127 bool UnorderedPairs<T>::contains(const T* left, const T* right) const
128 {
129  const auto* less = left;
130  const auto* greater = right;
131 
132  if (less > greater)
133  std::swap(less, greater);
134 
135  const auto resultLeft = mList.find(less);
136  const bool foundLeft = (resultLeft != mList.end());
137  if (foundLeft)
138  {
139  auto& associatedRights = resultLeft->second;
140 
141  const auto resultRight = associatedRights.find(greater);
142  const bool foundRight = (resultRight != associatedRights.end());
143  if (foundRight)
144  return true;
145  }
146 
147  return false;
148 }
149 
150 } // namespace detail
151 } // namespace collision
152 } // namespace dart
153 
154 #endif // DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
Definition: UnorderedPairs.hpp:45
void removeAllPairs()
Removes all the pairs from this container.
Definition: UnorderedPairs.hpp:120
std::unordered_map< const T *, std::unordered_set< const T * > > mList
The actual container to store pairs.
Definition: UnorderedPairs.hpp:65
void removePair(const T *left, const T *right)
Removes a pair from this container.
Definition: UnorderedPairs.hpp:94
void addPair(const T *left, const T *right)
Adds a pair to this container.
Definition: UnorderedPairs.hpp:70
bool contains(const T *left, const T *right) const
Returns true if this container contains the pair.
Definition: UnorderedPairs.hpp:127
Definition: BulletCollisionDetector.cpp:63