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 36 /** @author Jia Pan */ 37 38 #ifndef FCL_BROAD_PHASE_SSAP_H 39 #define FCL_BROAD_PHASE_SSAP_H 40 41 #include <vector> 42 #include "fcl/broadphase/broadphase_collision_manager.h" 43 44 namespace fcl 45 { 46 47 /// @brief Simple SAP collision manager 48 template <typename S> 49 class FCL_EXPORT SSaPCollisionManager : public BroadPhaseCollisionManager<S> 50 { 51 public: 52 SSaPCollisionManager(); 53 54 /// @brief remove one object from the manager 55 void registerObject(CollisionObject<S>* obj); 56 57 /// @brief add one object to the manager 58 void unregisterObject(CollisionObject<S>* obj); 59 60 /// @brief initialize the manager, related with the specific type of manager 61 void setup(); 62 63 /// @brief update the condition of manager 64 void update(); 65 66 /// @brief clear the manager 67 void clear(); 68 69 /// @brief return the objects managed by the manager 70 void getObjects(std::vector<CollisionObject<S>*>& objs) const; 71 72 /// @brief perform collision test between one object and all the objects belonging to the manager 73 void collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const; 74 75 /// @brief perform distance computation between one object and all the objects belonging to the manager 76 void distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const; 77 78 /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) 79 void collide(void* cdata, CollisionCallBack<S> callback) const; 80 81 /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) 82 void distance(void* cdata, DistanceCallBack<S> callback) const; 83 84 /// @brief perform collision test with objects belonging to another manager 85 void collide(BroadPhaseCollisionManager<S>* other_manager, void* cdata, CollisionCallBack<S> callback) const; 86 87 /// @brief perform distance test with objects belonging to another manager 88 void distance(BroadPhaseCollisionManager<S>* other_manager, void* cdata, DistanceCallBack<S> callback) const; 89 90 /// @brief whether the manager is empty 91 bool empty() const; 92 93 /// @brief the number of objects managed by the manager 94 size_t size() const; 95 96 protected: 97 /// @brief check collision between one object and a list of objects, return value is whether stop is possible 98 bool checkColl(typename std::vector<CollisionObject<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end, 99 CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const; 100 101 /// @brief check distance between one object and a list of objects, return value is whether stop is possible 102 bool checkDis(typename std::vector<CollisionObject<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end, 103 CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const; 104 105 bool collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const; 106 107 bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const; 108 109 static size_t selectOptimalAxis( 110 const std::vector<CollisionObject<S>*>& objs_x, 111 const std::vector<CollisionObject<S>*>& objs_y, 112 const std::vector<CollisionObject<S>*>& objs_z, 113 typename std::vector<CollisionObject<S>*>::const_iterator& it_beg, 114 typename std::vector<CollisionObject<S>*>::const_iterator& it_end); 115 116 /// @brief Objects sorted according to lower x value 117 std::vector<CollisionObject<S>*> objs_x; 118 119 /// @brief Objects sorted according to lower y value 120 std::vector<CollisionObject<S>*> objs_y; 121 122 /// @brief Objects sorted according to lower z value 123 std::vector<CollisionObject<S>*> objs_z; 124 125 /// @brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly 126 bool setup_; 127 }; 128 129 using SSaPCollisionManagerf = SSaPCollisionManager<float>; 130 using SSaPCollisionManagerd = SSaPCollisionManager<double>; 131 132 } // namespace fcl 133 134 #include "fcl/broadphase/broadphase_SSaP-inl.h" 135 136 #endif 137