hpp-fcl  1.4.4
HPP fork of FCL -- The Flexible Collision Library
geometric_shapes_utility.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 
39 #ifndef HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H
40 #define HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H
41 
42 #include <vector>
43 #include <hpp/fcl/shape/geometric_shapes.h>
44 #include <hpp/fcl/BV/BV.h>
45 #include <hpp/fcl/internal/BV_fitter.h>
46 
47 namespace hpp
48 {
49 namespace fcl
50 {
51 
53 namespace details
54 {
56 std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf);
57 std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf);
58 std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf);
59 std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
60 std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf);
61 std::vector<Vec3f> getBoundVertices(const ConvexBase& convex, const Transform3f& tf);
62 std::vector<Vec3f> getBoundVertices(const TriangleP& triangle, const Transform3f& tf);
63 }
65 
66 
68 template<typename BV, typename S>
69 void computeBV(const S& s, const Transform3f& tf, BV& bv)
70 {
71  std::vector<Vec3f> convex_bound_vertices = details::getBoundVertices(s, tf);
72  fit(&convex_bound_vertices[0], (int)convex_bound_vertices.size(), bv);
73 }
74 
75 template<>
76 void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv);
77 
78 template<>
79 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv);
80 
81 template<>
82 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv);
83 
84 template<>
85 void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv);
86 
87 template<>
88 void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv);
89 
90 template<>
91 void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf, AABB& bv);
92 
93 template<>
94 void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf, AABB& bv);
95 
96 template<>
97 void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv);
98 
99 template<>
100 void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv);
101 
102 
103 
104 template<>
105 void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv);
106 
107 template<>
108 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv);
109 
110 template<>
111 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv);
112 
113 template<>
114 void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv);
115 
116 template<>
117 void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv);
118 
119 template<>
120 void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf, OBB& bv);
121 
122 template<>
123 void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv);
124 
125 template<>
126 void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv);
127 
128 template<>
129 void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv);
130 
131 template<>
132 void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv);
133 
134 template<>
135 void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv);
136 
137 template<>
138 void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv);
139 
140 template<>
141 void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv);
142 
143 template<>
144 void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
145 
146 template<>
147 void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv);
148 
149 template<>
150 void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv);
151 
152 template<>
153 void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv);
154 
155 template<>
156 void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv);
157 
158 template<>
159 void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv);
160 
161 template<>
162 void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv);
163 
164 
166 void constructBox(const AABB& bv, Box& box, Transform3f& tf);
167 
168 void constructBox(const OBB& bv, Box& box, Transform3f& tf);
169 
170 void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf);
171 
172 void constructBox(const kIOS& bv, Box& box, Transform3f& tf);
173 
174 void constructBox(const RSS& bv, Box& box, Transform3f& tf);
175 
176 void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf);
177 
178 void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf);
179 
180 void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf);
181 
182 void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
183 
184 void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
185 
186 void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
187 
188 void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
189 
190 void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
191 
192 void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
193 
194 void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
195 
196 void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
197 
198 Halfspace transform(const Halfspace& a, const Transform3f& tf);
199 
200 Plane transform(const Plane& a, const Transform3f& tf);
201 
202 }
203 
204 } // namespace hpp
205 
206 #endif
hpp::fcl::computeBV< OBB, ConvexBase >
void computeBV< OBB, ConvexBase >(const ConvexBase &s, const Transform3f &tf, OBB &bv)
hpp::fcl::computeBV< AABB, Box >
void computeBV< AABB, Box >(const Box &s, const Transform3f &tf, AABB &bv)
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::computeBV< AABB, Plane >
void computeBV< AABB, Plane >(const Plane &s, const Transform3f &tf, AABB &bv)
hpp::fcl::computeBV< OBBRSS, Plane >
void computeBV< OBBRSS, Plane >(const Plane &s, const Transform3f &tf, OBBRSS &bv)
hpp::fcl::computeBV
void computeBV(const S &s, const Transform3f &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometric_shapes_utility.h:69
hpp::fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23.
Definition: kDOP.h:89
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:55
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:243
hpp
Main namespace.
Definition: AABB.h:43
hpp::fcl::computeBV< OBB, Sphere >
void computeBV< OBB, Sphere >(const Sphere &s, const Transform3f &tf, OBB &bv)
hpp::fcl::OBB
Oriented bounding box class.
Definition: OBB.h:54
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:337
hpp::fcl::computeBV< OBB, Capsule >
void computeBV< OBB, Capsule >(const Capsule &s, const Transform3f &tf, OBB &bv)
hpp::fcl::computeBV< OBB, Cylinder >
void computeBV< OBB, Cylinder >(const Cylinder &s, const Transform3f &tf, OBB &bv)
hpp::fcl::computeBV< OBB, Cone >
void computeBV< OBB, Cone >(const Cone &s, const Transform3f &tf, OBB &bv)
hpp::fcl::computeBV< OBB, Halfspace >
void computeBV< OBB, Halfspace >(const Halfspace &s, const Transform3f &tf, OBB &bv)
hpp::fcl::computeBV< OBB, Plane >
void computeBV< OBB, Plane >(const Plane &s, const Transform3f &tf, OBB &bv)
hpp::fcl::Plane
Infinite plane.
Definition: geometric_shapes.h:385
hpp::fcl::computeBV< AABB, TriangleP >
void computeBV< AABB, TriangleP >(const TriangleP &s, const Transform3f &tf, AABB &bv)
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:55
hpp::fcl::computeBV< RSS, Plane >
void computeBV< RSS, Plane >(const Plane &s, const Transform3f &tf, RSS &bv)
BV_fitter.h
details
Definition: traversal_node_setup.h:775
hpp::fcl::computeBV< AABB, Cylinder >
void computeBV< AABB, Cylinder >(const Cylinder &s, const Transform3f &tf, AABB &bv)
hpp::fcl::fit
void fit(Vec3f *ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: BV_fitter.h:54
hpp::fcl::computeBV< AABB, Sphere >
void computeBV< AABB, Sphere >(const Sphere &s, const Transform3f &tf, AABB &bv)
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:86
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:56
hpp::fcl::computeBV< kIOS, Halfspace >
void computeBV< kIOS, Halfspace >(const Halfspace &s, const Transform3f &tf, kIOS &bv)
hpp::fcl::computeBV< kIOS, Plane >
void computeBV< kIOS, Plane >(const Plane &s, const Transform3f &tf, kIOS &bv)
hpp::fcl::computeBV< AABB, Halfspace >
void computeBV< AABB, Halfspace >(const Halfspace &s, const Transform3f &tf, AABB &bv)
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:70
hpp::fcl::computeBV< OBB, Box >
void computeBV< OBB, Box >(const Box &s, const Transform3f &tf, OBB &bv)
geometric_shapes.h
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:199
hpp::fcl::computeBV< AABB, Cone >
void computeBV< AABB, Cone >(const Cone &s, const Transform3f &tf, AABB &bv)
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:55
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:122
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB...
Definition: geometric_shapes.h:154
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: geometric_shapes.h:281
hpp::fcl::computeBV< RSS, Halfspace >
void computeBV< RSS, Halfspace >(const Halfspace &s, const Transform3f &tf, RSS &bv)
hpp::fcl::computeBV< OBBRSS, Halfspace >
void computeBV< OBBRSS, Halfspace >(const Halfspace &s, const Transform3f &tf, OBBRSS &bv)
hpp::fcl::computeBV< AABB, ConvexBase >
void computeBV< AABB, ConvexBase >(const ConvexBase &s, const Transform3f &tf, AABB &bv)
hpp::fcl::constructBox
void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
hpp::fcl::transform
Halfspace transform(const Halfspace &a, const Transform3f &tf)
hpp::fcl::computeBV< AABB, Capsule >
void computeBV< AABB, Capsule >(const Capsule &s, const Transform3f &tf, AABB &bv)