broadphase_SaP.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
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  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef HPP_FCL_BROAD_PHASE_SAP_H
39 #define HPP_FCL_BROAD_PHASE_SAP_H
40 
41 #include <map>
42 #include <list>
43 
45 
46 namespace hpp {
47 namespace fcl {
48 
50 class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
51  public:
53  using Base::getObjects;
54 
56 
58 
60  void registerObjects(const std::vector<CollisionObject*>& other_objs);
61 
63  void registerObject(CollisionObject* obj);
64 
66  void unregisterObject(CollisionObject* obj);
67 
69  void setup();
70 
72  virtual void update();
73 
75  void update(CollisionObject* updated_obj);
76 
78  void update(const std::vector<CollisionObject*>& updated_objs);
79 
81  void clear();
82 
84  void getObjects(std::vector<CollisionObject*>& objs) const;
85 
89 
93 
97 
101 
103  void collide(BroadPhaseCollisionManager* other_manager,
105 
107  void distance(BroadPhaseCollisionManager* other_manager,
109 
111  bool empty() const;
112 
114  size_t size() const;
115 
116  protected:
117  struct EndPoint;
118 
120  struct SaPAABB {
123 
126 
129 
132  };
133 
135  struct EndPoint {
138  char minmax;
139 
142 
144  EndPoint* prev[3];
145 
147  EndPoint* next[3];
148 
150  const Vec3f& getVal() const;
151 
153  Vec3f& getVal();
154 
155  FCL_REAL getVal(size_t i) const;
156 
157  FCL_REAL& getVal(size_t i);
158  };
159 
162  struct SaPPair {
164 
167 
168  bool operator==(const SaPPair& other) const;
169  };
170 
172  class HPP_FCL_DLLAPI isUnregistered {
174 
175  public:
177 
178  bool operator()(const SaPPair& pair) const;
179  };
180 
183  class HPP_FCL_DLLAPI isNotValidPair {
186 
187  public:
189 
190  bool operator()(const SaPPair& pair);
191  };
192 
193  void update_(SaPAABB* updated_aabb);
194 
195  void updateVelist();
196 
198  EndPoint* elist[3];
199 
201  std::vector<EndPoint*> velist[3];
202 
204  std::list<SaPAABB*> AABB_arr;
205 
207  std::list<SaPPair> overlap_pairs;
208 
210 
211  std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
212 
213  bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
214  FCL_REAL& min_dist) const;
215 
216  bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
217 
218  void addToOverlapPairs(const SaPPair& p);
219 
220  void removeFromOverlapPairs(const SaPPair& p);
221 };
222 
223 } // namespace fcl
224 } // namespace hpp
225 
226 #endif
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Main namespace.
Base callback class for collision queries. This class can be supersed by child classes to provide des...
std::list< SaPPair > overlap_pairs
The pair of objects that should further check for collision.
SAP interval for one object.
std::map< CollisionObject *, SaPAABB * > obj_aabb_map
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
double FCL_REAL
Definition: data_types.h:65
Rigorous SAP collision manager.
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
EndPoint * hi
higher bound end point of the interval
std::list< SaPAABB * > AABB_arr
SAP interval list.
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Functor to help remove collision pairs no longer valid (i.e., should be culled away) ...
BroadPhaseCollisionManager Base
Functor to help unregister one object.
list a
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
EndPoint * lo
lower bound end point of the interval
the object for collision or distance computation, contains the geometry and the transform information...
A pair of objects that are not culling away and should further check collision.
char minmax
tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi ...
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
SaPAABB * aabb
back pointer to SAP interval


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00