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_KIOS_H 39 #define FCL_KIOS_H 40 41 #include "fcl/BV/OBB.h" 42 43 44 namespace fcl 45 { 46 47 /// @brief A class describing the kIOS collision structure, which is a set of spheres. 48 class kIOS 49 { 50 /// @brief One sphere in kIOS 51 struct kIOS_Sphere 52 { 53 Vec3f o; 54 FCL_REAL r; 55 }; 56 57 /// @brief generate one sphere enclosing two spheres encloseSphere(const kIOS_Sphere & s0,const kIOS_Sphere & s1)58 static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) 59 { 60 Vec3f d = s1.o - s0.o; 61 FCL_REAL dist2 = d.sqrLength(); 62 FCL_REAL diff_r = s1.r - s0.r; 63 64 /** The sphere with the larger radius encloses the other */ 65 if(diff_r * diff_r >= dist2) 66 { 67 if(s1.r > s0.r) return s1; 68 else return s0; 69 } 70 else /** spheres partially overlapping or disjoint */ 71 { 72 float dist = std::sqrt(dist2); 73 kIOS_Sphere s; 74 s.r = dist + s0.r + s1.r; 75 if(dist > 0) 76 s.o = s0.o + d * ((s.r - s0.r) / dist); 77 else 78 s.o = s0.o; 79 return s; 80 } 81 } 82 public: 83 84 /// @brief The (at most) five spheres for intersection 85 kIOS_Sphere spheres[5]; 86 87 /// @brief The number of spheres, no larger than 5 88 unsigned int num_spheres; 89 90 /// @ OBB related with kIOS 91 OBB obb; 92 93 /// @brief Check collision between two kIOS 94 bool overlap(const kIOS& other) const; 95 96 /// @brief Check collision between two kIOS and return the overlap part. 97 /// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS 98 /// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling. overlap(const kIOS & other,kIOS & overlap_part)99 bool overlap(const kIOS& other, kIOS& overlap_part) const 100 { 101 return overlap(other); 102 } 103 104 /// @brief Check whether the kIOS contains a point 105 inline bool contain(const Vec3f& p) const; 106 107 /// @brief A simple way to merge the kIOS and a point 108 kIOS& operator += (const Vec3f& p); 109 110 /// @brief Merge the kIOS and another kIOS 111 kIOS& operator += (const kIOS& other) 112 { 113 *this = *this + other; 114 return *this; 115 } 116 117 /// @brief Return the merged kIOS of current kIOS and the other one 118 kIOS operator + (const kIOS& other) const; 119 120 /// @brief Center of the kIOS center()121 const Vec3f& center() const 122 { 123 return spheres[0].o; 124 } 125 126 /// @brief Width of the kIOS 127 FCL_REAL width() const; 128 129 /// @brief Height of the kIOS 130 FCL_REAL height() const; 131 132 /// @brief Depth of the kIOS 133 FCL_REAL depth() const; 134 135 /// @brief Volume of the kIOS 136 FCL_REAL volume() const; 137 138 /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) 139 FCL_REAL size() const; 140 141 /// @brief The distance between two kIOS 142 FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; 143 }; 144 145 146 /// @brief Translate the kIOS BV 147 kIOS translate(const kIOS& bv, const Vec3f& t); 148 149 /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. 150 /// @todo Not efficient 151 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2); 152 153 /// @brief Approximate distance between two kIOS bounding volumes 154 /// @todo P and Q is not returned, need implementation 155 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL); 156 157 } 158 159 #endif 160