hpp-fcl  1.7.3
HPP fork of FCL -- The Flexible Collision Library
kDOP.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_KDOP_H
39 #define HPP_FCL_KDOP_H
40 
41 #include <stdexcept>
42 #include <hpp/fcl/data_types.h>
43 
44 namespace hpp
45 {
46 namespace fcl
47 {
48 
49 struct CollisionRequest;
50 
53 
88 template<short N>
90 {
91 protected:
93  Eigen::Array<FCL_REAL, N, 1> dist_;
94 
95 public:
96 
98  KDOP();
99 
101  KDOP(const Vec3f& v);
102 
104  KDOP(const Vec3f& a, const Vec3f& b);
105 
107  bool operator==(const KDOP & other) const
108  {
109  return (dist_ == other.dist_).all();
110  }
111 
113  bool operator!=(const KDOP & other) const
114  {
115  return (dist_ != other.dist_).any();
116  }
117 
119  bool overlap(const KDOP<N>& other) const;
120 
125  bool overlap(const KDOP<N>& other, const CollisionRequest& request,
126  FCL_REAL& sqrDistLowerBound) const;
127 
129  FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
130 
132  KDOP<N>& operator += (const Vec3f& p);
133 
135  KDOP<N>& operator += (const KDOP<N>& other);
136 
138  KDOP<N> operator + (const KDOP<N>& other) const;
139 
141  inline FCL_REAL size() const
142  {
143  return width() * width() + height() * height() + depth() * depth();
144  }
145 
147  inline Vec3f center() const
148  {
149  return (dist_.template head<3>() + dist_.template segment<3>(N/2)) / 2;
150  }
151 
152 
154  inline FCL_REAL width() const
155  {
156  return dist_[N / 2] - dist_[0];
157  }
158 
160  inline FCL_REAL height() const
161  {
162  return dist_[N / 2 + 1] - dist_[1];
163  }
164 
166  inline FCL_REAL depth() const
167  {
168  return dist_[N / 2 + 2] - dist_[2];
169  }
170 
172  inline FCL_REAL volume() const
173  {
174  return width() * height() * depth();
175  }
176 
177  inline FCL_REAL dist(short i) const
178  {
179  return dist_[i];
180  }
181 
182  inline FCL_REAL& dist(short i)
183  {
184  return dist_[i];
185  }
186 
188  bool inside(const Vec3f& p) const;
189 
190  public:
192  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
194 };
195 
196 template<short N>
197 bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
198  const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
199 {
200  throw std::logic_error ("not implemented");
201 }
202 
203 template<short N>
204 bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
205  const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
206  const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
207 {
208  throw std::logic_error ("not implemented");
209 }
210 
212 template<short N>
213 HPP_FCL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
214 
215 } // namespace fcl
216 
217 } // namespace hpp
218 
219 #endif
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
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
bool operator!=(const KDOP &other) const
Difference operator.
Definition: kDOP.h:113
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
FCL_REAL depth() const
The (AABB) depth.
Definition: kDOP.h:166
request to the collision algorithm
Definition: collision_data.h:208
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.
FCL_REAL & dist(short i)
Definition: kDOP.h:182
double FCL_REAL
Definition: data_types.h:66
FCL_REAL height() const
The (AABB) height.
Definition: kDOP.h:160
bool operator==(const KDOP &other) const
Equality operator.
Definition: kDOP.h:107
Eigen::Array< FCL_REAL, N, 1 > dist_
Origin&#39;s distances to N KDOP planes.
Definition: kDOP.h:93
FCL_REAL dist(short i) const
Definition: kDOP.h:177
FCL_REAL volume() const
The (AABB) volume.
Definition: kDOP.h:172
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.
Vec3f center() const
The (AABB) center.
Definition: kDOP.h:147
FCL_REAL size() const
Size of the kDOP (used in BV_Splitter to order two kDOPs)
Definition: kDOP.h:141
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
FCL_REAL width() const
The (AABB) width.
Definition: kDOP.h:154
#define HPP_FCL_DLLAPI
Definition: config.hh:64