hpp-fcl: include/hpp/fcl/BV/kIOS.h Source File

hpp-fcl 1.7.0 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 HPP_FCL_DLLAPI kIOS
56 {
58 struct HPP_FCL_DLLAPI kIOS_Sphere
59 {
60 Vec3f o;
61 FCL_REAL r;
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
130 OBB obb;
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
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:56
hpp::fcl::kIOS::operator!=
bool operator!=(const kIOS &other) const
Difference operator.
Definition: kIOS.h:118
hpp::fcl::kIOS::depth
FCL_REAL depth() const
Depth of the kIOS.
hpp::fcl::kIOS::spheres
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:124
hpp::fcl::kIOS::center
const Vec3f & center() const
Center of the kIOS.
Definition: kIOS.h:162
hpp::fcl::distance
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.
hpp::fcl::kIOS::kIOS_Sphere::operator==
bool operator==(const kIOS_Sphere &other) const
Definition: kIOS.h:63
hpp::fcl::kIOS::overlap
bool overlap(const kIOS &other) const
Check collision between two kIOS.
hpp::fcl::kIOS::width
FCL_REAL width() const
Width of the kIOS.
hpp::fcl::kIOS::overlap
bool overlap(const kIOS &other, const CollisionRequest &, FCL_REAL &sqrDistLowerBound) const
Check collision between two kIOS.
hpp::fcl::kIOS::height
FCL_REAL height() const
Height of the kIOS.
hpp::fcl::kIOS::kIOS_Sphere::r
FCL_REAL r
Definition: kIOS.h:61
hpp::fcl::overlap
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.
hpp::fcl::kIOS::volume
FCL_REAL volume() const
Volume of the kIOS.
hpp::fcl::kIOS::contain
bool contain(const Vec3f &p) const
Check whether the kIOS contains a point.
hpp::fcl::kIOS::size
FCL_REAL size() const
size of the kIOS (used in BV_Splitter to order two kIOSs)
hpp::fcl::kIOS::distance
FCL_REAL distance(const kIOS &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
The distance between two kIOS.
hpp::fcl::kIOS::operator==
bool operator==(const kIOS &other) const
Equality operator.
Definition: kIOS.h:102
hpp::fcl::kIOS::num_spheres
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:127
hpp::fcl::kIOS::obb
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:130
hpp::fcl::kIOS::kIOS_Sphere::o
Vec3f o
Definition: kIOS.h:60
hpp::fcl::kIOS::kIOS_Sphere::operator!=
bool operator!=(const kIOS_Sphere &other) const
Definition: kIOS.h:68
hpp::fcl::translate
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
OBB.h
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:204
hpp::fcl::OBB
Oriented bounding box class.
Definition: OBB.h:55