hpp-fcl  1.8.1
HPP fork of FCL -- The Flexible Collision Library
kIOS.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-2015, 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_KIOS_H
39 #define HPP_FCL_KIOS_H
40 
41 #include <hpp/fcl/BV/OBB.h>
42 
43 
44 namespace hpp
45 {
46 namespace fcl
47 {
48 
49 struct CollisionRequest;
50 
53 
56 {
58  struct HPP_FCL_DLLAPI kIOS_Sphere
59  {
62 
63  bool operator==(const kIOS_Sphere & other) const
64  {
65  return o == other.o && r == other.r;
66  }
67 
68  bool operator!=(const kIOS_Sphere & other) const
69  {
70  return !(*this == other);
71  }
72  };
73 
75  static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
76  {
77  Vec3f d = s1.o - s0.o;
78  FCL_REAL dist2 = d.squaredNorm();
79  FCL_REAL diff_r = s1.r - s0.r;
80 
82  if(diff_r * diff_r >= dist2)
83  {
84  if(s1.r > s0.r) return s1;
85  else return s0;
86  }
87  else
88  {
89  float dist = (float)std::sqrt(dist2);
90  kIOS_Sphere s;
91  s.r = dist + s0.r + s1.r;
92  if(dist > 0)
93  s.o = s0.o + d * ((s.r - s0.r) / dist);
94  else
95  s.o = s0.o;
96  return s;
97  }
98  }
99 public:
100 
102  bool operator==(const kIOS & other) const
103  {
104  bool res = obb == other.obb && num_spheres == other.num_spheres;
105  if(!res)
106  return false;
107 
108  for(size_t k = 0; k < num_spheres; ++k)
109  {
110  if(spheres[k] != other.spheres[k])
111  return false;
112  }
113 
114  return true;
115  }
116 
118  bool operator!=(const kIOS & other) const
119  {
120  return !(*this == other);
121  }
122 
124  kIOS_Sphere spheres[5];
125 
127  unsigned int num_spheres;
128 
131 
133  bool contain(const Vec3f& p) const;
134 
136  bool overlap(const kIOS& other) const;
137 
139  bool overlap(const kIOS& other, const CollisionRequest&,
140  FCL_REAL& sqrDistLowerBound) const;
141 
143  FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
144 
146  kIOS& operator += (const Vec3f& p);
147 
149  kIOS& operator += (const kIOS& other)
150  {
151  *this = *this + other;
152  return *this;
153  }
154 
156  kIOS operator + (const kIOS& other) const;
157 
159  FCL_REAL size() const;
160 
162  const Vec3f& center() const
163  {
164  return spheres[0].o;
165  }
166 
168  FCL_REAL width() const;
169 
171  FCL_REAL height() const;
172 
174  FCL_REAL depth() const;
175 
177  FCL_REAL volume() const;
178 };
179 
180 
182 HPP_FCL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t);
183 
186 HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0,
187  const kIOS& b1, const kIOS& b2);
188 
191 HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0,
192  const kIOS& b1, const kIOS& b2,
193  const CollisionRequest& request,
194  FCL_REAL& sqrDistLowerBound);
195 
198 HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0,
199  const kIOS& b1, const kIOS& b2,
200  Vec3f* P = NULL, Vec3f* Q = NULL);
201 
202 }
203 
204 } // namespace hpp
205 
206 #endif
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:124
Main namespace.
Definition: AABB.h:43
bool operator!=(const kIOS_Sphere &other) const
Definition: kIOS.h:68
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
FCL_REAL r
Definition: kIOS.h:61
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:130
bool operator==(const kIOS &other) const
Equality operator.
Definition: kIOS.h:102
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:127
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:55
request to the collision algorithm
Definition: collision_data.h:209
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.
double FCL_REAL
Definition: data_types.h:66
const Vec3f & center() const
Center of the kIOS.
Definition: kIOS.h:162
bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool operator!=(const kIOS &other) const
Difference operator.
Definition: kIOS.h:118
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
bool operator==(const kIOS_Sphere &other) const
Definition: kIOS.h:63
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Vec3f o
Definition: kIOS.h:60
Oriented bounding box class.
Definition: OBB.h:54