hpp-fcl  1.4.4
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 
55 class kIOS
56 {
58  struct kIOS_Sphere
59  {
62  };
63 
65  static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
66  {
67  Vec3f d = s1.o - s0.o;
68  FCL_REAL dist2 = d.squaredNorm();
69  FCL_REAL diff_r = s1.r - s0.r;
70 
72  if(diff_r * diff_r >= dist2)
73  {
74  if(s1.r > s0.r) return s1;
75  else return s0;
76  }
77  else
78  {
79  float dist = (float)std::sqrt(dist2);
80  kIOS_Sphere s;
81  s.r = dist + s0.r + s1.r;
82  if(dist > 0)
83  s.o = s0.o + d * ((s.r - s0.r) / dist);
84  else
85  s.o = s0.o;
86  return s;
87  }
88  }
89 public:
90 
92  kIOS_Sphere spheres[5];
93 
95  unsigned int num_spheres;
96 
99 
101  bool contain(const Vec3f& p) const;
102 
104  bool overlap(const kIOS& other) const;
105 
107  bool overlap(const kIOS& other, const CollisionRequest&,
108  FCL_REAL& sqrDistLowerBound) const;
109 
111  FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
112 
114  kIOS& operator += (const Vec3f& p);
115 
117  kIOS& operator += (const kIOS& other)
118  {
119  *this = *this + other;
120  return *this;
121  }
122 
124  kIOS operator + (const kIOS& other) const;
125 
127  FCL_REAL size() const;
128 
130  const Vec3f& center() const
131  {
132  return spheres[0].o;
133  }
134 
136  FCL_REAL width() const;
137 
139  FCL_REAL height() const;
140 
142  FCL_REAL depth() const;
143 
145  FCL_REAL volume() const;
146 };
147 
148 
150 kIOS translate(const kIOS& bv, const Vec3f& t);
151 
154 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
155 
158 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
159  const CollisionRequest& request,
160  FCL_REAL& sqrDistLowerBound);
161 
164 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
165 
166 }
167 
168 } // namespace hpp
169 
170 #endif
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:92
FCL_REAL width() const
Width of the kIOS.
FCL_REAL size() const
size of the kIOS (used in BV_Splitter to order two kIOSs)
Main namespace.
Definition: AABB.h:43
Oriented bounding box class.
Definition: OBB.h:54
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
FCL_REAL r
Definition: kIOS.h:61
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
kIOS & operator+=(const Vec3f &p)
A simple way to merge the kIOS and a point.
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:98
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:95
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:150
double FCL_REAL
Definition: data_types.h:68
FCL_REAL height() const
Height of the kIOS.
FCL_REAL volume() const
Volume of the kIOS.
const Vec3f & center() const
Center of the kIOS.
Definition: kIOS.h:130
FCL_REAL depth() const
Depth of the kIOS.
kIOS operator+(const kIOS &other) const
Return the merged kIOS of current kIOS and the other one.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
bool overlap(const kIOS &other) const
Check collision between two kIOS.
FCL_REAL distance(const kIOS &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
The distance between two kIOS.
bool contain(const Vec3f &p) const
Check whether the kIOS contains a point.
Vec3f o
Definition: kIOS.h:60