DART  6.10.1
UnorderedPairs.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, 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
85  = mList.insert(std::make_pair(less, std::unordered_set<const T*>()))
86  .first;
87 
88  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
89  // pair already existed, this will do nothing.
90  itLess->second.insert(greater);
91 }
92 
93 //==============================================================================
94 template <class T>
95 void UnorderedPairs<T>::removePair(const T* left, const T* right)
96 {
97  if (!left || !right)
98  return;
99 
100  const auto* bodyNodeLess = left;
101  const auto* bodyNodeGreater = right;
102 
103  if (bodyNodeLess > bodyNodeGreater)
104  std::swap(bodyNodeLess, bodyNodeGreater);
105 
106  // Remove the pair only when it already exists
107  const auto resultLeft = mList.find(bodyNodeLess);
108  const bool foundLeft = (resultLeft != mList.end());
109  if (foundLeft)
110  {
111  auto& associatedRights = resultLeft->second;
112  associatedRights.erase(bodyNodeGreater);
113 
114  if (associatedRights.empty())
115  mList.erase(resultLeft);
116  }
117 }
118 
119 //==============================================================================
120 template <class T>
122 {
123  mList.clear();
124 }
125 
126 //==============================================================================
127 template <class T>
128 bool UnorderedPairs<T>::contains(const T* left, const T* right) const
129 {
130  const auto* less = left;
131  const auto* greater = right;
132 
133  if (less > greater)
134  std::swap(less, greater);
135 
136  const auto resultLeft = mList.find(less);
137  const bool foundLeft = (resultLeft != mList.end());
138  if (foundLeft)
139  {
140  auto& associatedRights = resultLeft->second;
141 
142  const auto resultRight = associatedRights.find(greater);
143  const bool foundRight = (resultRight != associatedRights.end());
144  if (foundRight)
145  return true;
146  }
147 
148  return false;
149 }
150 
151 } // namespace detail
152 } // namespace collision
153 } // namespace dart
154 
155 #endif // DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
Definition: UnorderedPairs.hpp:45
void removeAllPairs()
Removes all the pairs from this container.
Definition: UnorderedPairs.hpp:121
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:95
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:128
Definition: BulletCollisionDetector.cpp:65