hpp-fcl 2.2.0
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
43namespace hpp {
44namespace fcl {
45
46struct CollisionRequest;
47
50
55 struct HPP_FCL_DLLAPI kIOS_Sphere {
58
59 bool operator==(const kIOS_Sphere& other) const {
60 return o == other.o && r == other.r;
61 }
62
63 bool operator!=(const kIOS_Sphere& other) const {
64 return !(*this == other);
65 }
66 };
67
69 static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0,
70 const kIOS_Sphere& s1) {
71 Vec3f d = s1.o - s0.o;
72 FCL_REAL dist2 = d.squaredNorm();
73 FCL_REAL diff_r = s1.r - s0.r;
74
76 if (diff_r * diff_r >= dist2) {
77 if (s1.r > s0.r)
78 return s1;
79 else
80 return s0;
81 } else
82 {
83 float dist = (float)std::sqrt(dist2);
84 kIOS_Sphere s;
85 s.r = dist + s0.r + s1.r;
86 if (dist > 0)
87 s.o = s0.o + d * ((s.r - s0.r) / dist);
88 else
89 s.o = s0.o;
90 return s;
91 }
92 }
93
94 public:
96 bool operator==(const kIOS& other) const {
97 bool res = obb == other.obb && num_spheres == other.num_spheres;
98 if (!res) return false;
99
100 for (size_t k = 0; k < num_spheres; ++k) {
101 if (spheres[k] != other.spheres[k]) return false;
102 }
103
104 return true;
105 }
106
108 bool operator!=(const kIOS& other) const { return !(*this == other); }
109
111 kIOS_Sphere spheres[5];
112
114 unsigned int num_spheres;
115
118
120 bool contain(const Vec3f& p) const;
121
123 bool overlap(const kIOS& other) const;
124
126 bool overlap(const kIOS& other, const CollisionRequest&,
127 FCL_REAL& sqrDistLowerBound) const;
128
130 FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
131
134
136 kIOS& operator+=(const kIOS& other) {
137 *this = *this + other;
138 return *this;
139 }
140
142 kIOS operator+(const kIOS& other) const;
143
145 FCL_REAL size() const;
146
148 const Vec3f& center() const { return spheres[0].o; }
149
152
155
158
161};
162
165
169HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
170 const kIOS& b2);
171
175HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
176 const kIOS& b2, const CollisionRequest& request,
177 FCL_REAL& sqrDistLowerBound);
178
182 const kIOS& b1, const kIOS& b2,
183 Vec3f* P = NULL, Vec3f* Q = NULL);
184
185} // namespace fcl
186
187} // namespace hpp
188
189#endif
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
#define HPP_FCL_DLLAPI
Definition: config.hh:64
bool operator!=(const kIOS &other) const
Difference operator.
Definition: kIOS.h:108
kIOS & operator+=(const kIOS &other)
Merge the kIOS and another kIOS.
Definition: kIOS.h:136
FCL_REAL depth() const
Depth of the kIOS.
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:111
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:59
bool overlap(const kIOS &other) const
Check collision between two kIOS.
kIOS & operator+=(const Vec3f &p)
A simple way to merge the kIOS and a point.
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:57
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:96
const Vec3f & center() const
Center of the kIOS.
Definition: kIOS.h:148
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:114
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:117
Vec3f o
Definition: kIOS.h:56
bool operator!=(const kIOS_Sphere &other) const
Definition: kIOS.h:63
kIOS operator+(const kIOS &other) const
Return the merged kIOS of current kIOS and the other one.
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
request to the collision algorithm
Definition: collision_data.h:235
Oriented bounding box class.
Definition: OBB.h:52