hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
44namespace hpp
45{
46namespace fcl
47{
48
49struct 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 }
99public:
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
169
172
175
178};
179
180
183
186HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0,
187 const kIOS& b1, const kIOS& b2);
188
191HPP_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
199 const kIOS& b1, const kIOS& b2,
200 Vec3f* P = NULL, Vec3f* Q = NULL);
201
202}
203
204} // namespace hpp
205
206#endif
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:56
#define HPP_FCL_DLLAPI
Definition: config.hh:64
bool operator!=(const kIOS &other) const
Difference operator.
Definition: kIOS.h:118
FCL_REAL depth() const
Depth of the kIOS.
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:124
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.
bool operator==(const kIOS_Sphere &other) const
Definition: kIOS.h:63
bool overlap(const kIOS &other) const
Check collision between two kIOS.
FCL_REAL width() const
Width of the kIOS.
bool overlap(const kIOS &other, const CollisionRequest &, FCL_REAL &sqrDistLowerBound) const
Check collision between two kIOS.
FCL_REAL height() const
Height of the kIOS.
FCL_REAL r
Definition: kIOS.h:61
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
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.
FCL_REAL volume() const
Volume of the kIOS.
bool contain(const Vec3f &p) const
Check whether the kIOS contains a point.
FCL_REAL size() const
size of the kIOS (used in BV_Splitter to order two kIOSs)
FCL_REAL distance(const kIOS &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
The distance between two kIOS.
bool operator==(const kIOS &other) const
Equality operator.
Definition: kIOS.h:102
const Vec3f & center() const
Center of the kIOS.
Definition: kIOS.h:162
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:127
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:130
Vec3f o
Definition: kIOS.h:60
bool operator!=(const kIOS_Sphere &other) const
Definition: kIOS.h:68
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44
request to the collision algorithm
Definition: collision_data.h:210
Oriented bounding box class.
Definition: OBB.h:55