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_TRAVERSAL_MESHCOLLISIONTRAVERSALNODE_H 39 #define FCL_TRAVERSAL_MESHCOLLISIONTRAVERSALNODE_H 40 41 #include "fcl/math/bv/OBB.h" 42 #include "fcl/math/bv/RSS.h" 43 #include "fcl/math/bv/OBBRSS.h" 44 #include "fcl/math/bv/kIOS.h" 45 #include "fcl/narrowphase/contact.h" 46 #include "fcl/narrowphase/cost_source.h" 47 #include "fcl/narrowphase/detail/traversal/collision/intersect.h" 48 #include "fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h" 49 50 namespace fcl 51 { 52 53 namespace detail 54 { 55 56 /// @brief Traversal node for collision between two meshes 57 template <typename BV> 58 class FCL_EXPORT MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> 59 { 60 public: 61 62 using S = typename BV::S; 63 64 MeshCollisionTraversalNode(); 65 66 /// @brief Intersection testing between leaves (two triangles) 67 void leafTesting(int b1, int b2) const; 68 69 /// @brief Whether the traversal process can stop early 70 bool canStop() const; 71 72 Vector3<S>* vertices1; 73 Vector3<S>* vertices2; 74 75 Triangle* tri_indices1; 76 Triangle* tri_indices2; 77 78 S cost_density; 79 }; 80 81 /// @brief Initialize traversal node for collision between two meshes, given the 82 /// current transforms 83 template <typename BV> 84 FCL_EXPORT 85 bool initialize( 86 MeshCollisionTraversalNode<BV>& node, 87 BVHModel<BV>& model1, 88 Transform3<typename BV::S>& tf1, 89 BVHModel<BV>& model2, 90 Transform3<typename BV::S>& tf2, 91 const CollisionRequest<typename BV::S>& request, 92 CollisionResult<typename BV::S>& result, 93 bool use_refit = false, 94 bool refit_bottomup = false); 95 96 /// @brief Traversal node for collision between two meshes if their underlying 97 /// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) 98 template <typename S> 99 class FCL_EXPORT MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB<S>> 100 { 101 public: 102 MeshCollisionTraversalNodeOBB(); 103 104 bool BVTesting(int b1, int b2) const; 105 106 void leafTesting(int b1, int b2) const; 107 108 bool BVTesting(int b1, int b2, const Matrix3<S>& Rc, const Vector3<S>& Tc) const; 109 110 bool BVTesting(int b1, int b2, const Transform3<S>& tf) const; 111 112 void leafTesting(int b1, int b2, const Matrix3<S>& Rc, const Vector3<S>& Tc) const; 113 114 void leafTesting(int b1, int b2, const Transform3<S>& tf) const; 115 116 Matrix3<S> R; 117 Vector3<S> T; 118 119 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 120 }; 121 122 using MeshCollisionTraversalNodeOBBf = MeshCollisionTraversalNodeOBB<float>; 123 using MeshCollisionTraversalNodeOBBd = MeshCollisionTraversalNodeOBB<double>; 124 125 /// @brief Initialize traversal node for collision between two meshes, 126 /// specialized for OBB type 127 template <typename S> 128 FCL_EXPORT 129 bool initialize( 130 MeshCollisionTraversalNodeOBB<S>& node, 131 const BVHModel<OBB<S>>& model1, 132 const Transform3<S>& tf1, 133 const BVHModel<OBB<S>>& model2, 134 const Transform3<S>& tf2, 135 const CollisionRequest<S>& request, 136 CollisionResult<S>& result); 137 138 template <typename S> 139 class FCL_EXPORT MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS<S>> 140 { 141 public: 142 MeshCollisionTraversalNodeRSS(); 143 144 bool BVTesting(int b1, int b2) const; 145 146 void leafTesting(int b1, int b2) const; 147 148 // FCL_DEPRECATED 149 // bool BVTesting(int b1, int b2, const Matrix3<S>& Rc, const Vector3<S>& Tc) const; 150 151 bool BVTesting(int b1, int b2, const Transform3<S>& tf) const; 152 153 // FCL_DEPRECATED 154 // void leafTesting(int b1, int b2, const Matrix3<S>& Rc, const Vector3<S>& Tc) const; 155 156 void leafTesting(int b1, int b2, const Transform3<S>& tf) const; 157 158 Matrix3<S> R; 159 Vector3<S> T; 160 161 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 162 }; 163 164 using MeshCollisionTraversalNodeRSSf = MeshCollisionTraversalNodeRSS<float>; 165 using MeshCollisionTraversalNodeRSSd = MeshCollisionTraversalNodeRSS<double>; 166 167 /// @brief Initialize traversal node for collision between two meshes, 168 /// specialized for RSS type 169 template <typename S> 170 FCL_EXPORT 171 bool initialize( 172 MeshCollisionTraversalNodeRSS<S>& node, 173 const BVHModel<RSS<S>>& model1, 174 const Transform3<S>& tf1, 175 const BVHModel<RSS<S>>& model2, 176 const Transform3<S>& tf2, 177 const CollisionRequest<S>& request, 178 CollisionResult<S>& result); 179 180 template <typename S> 181 class FCL_EXPORT MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS<S>> 182 { 183 public: 184 MeshCollisionTraversalNodekIOS(); 185 186 bool BVTesting(int b1, int b2) const; 187 188 void leafTesting(int b1, int b2) const; 189 190 Matrix3<S> R; 191 Vector3<S> T; 192 193 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 194 }; 195 196 using MeshCollisionTraversalNodekIOSf = MeshCollisionTraversalNodekIOS<float>; 197 using MeshCollisionTraversalNodekIOSd = MeshCollisionTraversalNodekIOS<double>; 198 199 /// @brief Initialize traversal node for collision between two meshes, 200 /// specialized for kIOS type 201 template <typename S> 202 FCL_EXPORT 203 bool initialize( 204 MeshCollisionTraversalNodekIOS<S>& node, 205 const BVHModel<kIOS<S>>& model1, 206 const Transform3<S>& tf1, 207 const BVHModel<kIOS<S>>& model2, 208 const Transform3<S>& tf2, 209 const CollisionRequest<S>& request, 210 CollisionResult<S>& result); 211 212 template <typename S> 213 class FCL_EXPORT MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS<S>> 214 { 215 public: 216 MeshCollisionTraversalNodeOBBRSS(); 217 218 219 bool BVTesting(int b1, int b2) const; 220 221 void leafTesting(int b1, int b2) const; 222 223 Matrix3<S> R; 224 Vector3<S> T; 225 226 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 227 }; 228 229 using MeshCollisionTraversalNodeOBBRSSf = MeshCollisionTraversalNodeOBBRSS<float>; 230 using MeshCollisionTraversalNodeOBBRSSd = MeshCollisionTraversalNodeOBBRSS<double>; 231 232 /// @brief Initialize traversal node for collision between two meshes, 233 /// specialized for OBBRSS type 234 template <typename S> 235 FCL_EXPORT 236 bool initialize( 237 MeshCollisionTraversalNodeOBBRSS<S>& node, 238 const BVHModel<OBBRSS<S>>& model1, 239 const Transform3<S>& tf1, 240 const BVHModel<OBBRSS<S>>& model2, 241 const Transform3<S>& tf2, 242 const CollisionRequest<S>& request, 243 CollisionResult<S>& result); 244 245 template <typename BV> 246 FCL_EXPORT 247 void meshCollisionOrientedNodeLeafTesting( 248 int b1, 249 int b2, 250 const BVHModel<BV>* model1, 251 const BVHModel<BV>* model2, 252 Vector3<typename BV::S>* vertices1, 253 Vector3<typename BV::S>* vertices2, 254 Triangle* tri_indices1, 255 Triangle* tri_indices2, 256 const Matrix3<typename BV::S>& R, 257 const Vector3<typename BV::S>& T, 258 const Transform3<typename BV::S>& tf1, 259 const Transform3<typename BV::S>& tf2, 260 bool enable_statistics, 261 typename BV::S cost_density, 262 int& num_leaf_tests, 263 const CollisionRequest<typename BV::S>& request, 264 CollisionResult<typename BV::S>& result); 265 266 template <typename BV> 267 FCL_EXPORT 268 void meshCollisionOrientedNodeLeafTesting( 269 int b1, 270 int b2, 271 const BVHModel<BV>* model1, 272 const BVHModel<BV>* model2, 273 Vector3<typename BV::S>* vertices1, 274 Vector3<typename BV::S>* vertices2, 275 Triangle* tri_indices1, 276 Triangle* tri_indices2, 277 const Transform3<typename BV::S>& tf, 278 const Transform3<typename BV::S>& tf1, 279 const Transform3<typename BV::S>& tf2, 280 bool enable_statistics, 281 typename BV::S cost_density, 282 int& num_leaf_tests, 283 const CollisionRequest<typename BV::S>& request, 284 CollisionResult<typename BV::S>& result); 285 286 } // namespace detail 287 } // namespace fcl 288 289 #include "fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h" 290 291 #endif 292