hpp-fcl  2.2.0
HPP fork of FCL -- The Flexible Collision Library
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 
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 
88  void collide(CollisionObject* obj, CollisionCallBackBase* callback) const;
89 
92  void distance(CollisionObject* obj, DistanceCallBackBase* callback) const;
93 
96  void collide(CollisionCallBackBase* callback) const;
97 
100  void distance(DistanceCallBackBase* callback) const;
101 
103  void collide(BroadPhaseCollisionManager* other_manager,
104  CollisionCallBackBase* callback) const;
105 
107  void distance(BroadPhaseCollisionManager* other_manager,
108  DistanceCallBackBase* callback) const;
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 
173  CollisionObject* obj;
174 
175  public:
177 
178  bool operator()(const SaPPair& pair) const;
179  };
180 
184  CollisionObject* obj1;
185  CollisionObject* obj2;
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...
Definition: broadphase_collision_manager.h:54
Main namespace.
Definition: broadphase_bruteforce.h:44
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
End point for an interval.
Definition: broadphase_SaP.h:135
std::list< SaPPair > overlap_pairs
The pair of objects that should further check for collision.
Definition: broadphase_SaP.h:207
SAP interval for one object.
Definition: broadphase_SaP.h:120
std::map< CollisionObject *, SaPAABB * > obj_aabb_map
Definition: broadphase_SaP.h:211
AABB cached
cached AABB value
Definition: broadphase_SaP.h:131
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.
int optimal_axis
Definition: broadphase_SaP.h:209
double FCL_REAL
Definition: data_types.h:65
CollisionObject * obj
object
Definition: broadphase_SaP.h:122
Rigorous SAP collision manager.
Definition: broadphase_SaP.h:50
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
EndPoint * hi
higher bound end point of the interval
Definition: broadphase_SaP.h:128
std::list< SaPAABB * > AABB_arr
SAP interval list.
Definition: broadphase_SaP.h:204
CollisionObject * obj2
Definition: broadphase_SaP.h:166
CollisionObject * obj1
Definition: broadphase_SaP.h:165
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) ...
Definition: broadphase_SaP.h:183
BroadPhaseCollisionManager Base
Definition: broadphase_SaP.h:52
Functor to help unregister one object.
Definition: broadphase_SaP.h:172
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
EndPoint * lo
lower bound end point of the interval
Definition: broadphase_SaP.h:125
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
A pair of objects that are not culling away and should further check collision.
Definition: broadphase_SaP.h:162
char minmax
tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi ...
Definition: broadphase_SaP.h:138
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
SaPAABB * aabb
back pointer to SAP interval
Definition: broadphase_SaP.h:141
#define HPP_FCL_DLLAPI
Definition: config.hh:64