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 <hpp/fcl/fwd.hh>
42 #include <hpp/fcl/data_types.h>
43 
44 namespace hpp {
45 namespace fcl {
46 
47 struct CollisionRequest;
48 
51 
91 template <short N>
92 class HPP_FCL_DLLAPI KDOP {
93  protected:
95  Eigen::Array<FCL_REAL, N, 1> dist_;
96 
97  public:
99  KDOP();
100 
102  KDOP(const Vec3f& v);
103 
105  KDOP(const Vec3f& a, const Vec3f& b);
106 
108  bool operator==(const KDOP& other) const {
109  return (dist_ == other.dist_).all();
110  }
111 
113  bool operator!=(const KDOP& other) const {
114  return (dist_ != other.dist_).any();
115  }
116 
118  bool overlap(const KDOP<N>& other) const;
119 
124  bool overlap(const KDOP<N>& other, const CollisionRequest& request,
125  FCL_REAL& sqrDistLowerBound) const;
126 
128  FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL,
129  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  return width() * width() + height() * height() + depth() * depth();
143  }
144 
146  inline Vec3f center() const {
147  return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
148  }
149 
151  inline FCL_REAL width() const { return dist_[N / 2] - dist_[0]; }
152 
154  inline FCL_REAL height() const { return dist_[N / 2 + 1] - dist_[1]; }
155 
157  inline FCL_REAL depth() const { return dist_[N / 2 + 2] - dist_[2]; }
158 
160  inline FCL_REAL volume() const { return width() * height() * depth(); }
161 
162  inline FCL_REAL dist(short i) const { return dist_[i]; }
163 
164  inline FCL_REAL& dist(short i) { return dist_[i]; }
165 
167  bool inside(const Vec3f& p) const;
168 
169  public:
171  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
173 };
174 
175 template <short N>
176 bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
177  const KDOP<N>& /*b2*/) {
178  HPP_FCL_THROW_PRETTY("not implemented", std::logic_error);
179 }
180 
181 template <short N>
182 bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
183  const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
184  FCL_REAL& /*sqrDistLowerBound*/) {
185  HPP_FCL_THROW_PRETTY("not implemented", std::logic_error);
186 }
187 
189 template <short N>
190 HPP_FCL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
191 
192 } // namespace fcl
193 
194 } // namespace hpp
195 
196 #endif
data_types.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::KDOP::operator!=
bool operator!=(const KDOP &other) const
Difference operator.
Definition: kDOP.h:113
hpp::fcl::KDOP::depth
FCL_REAL depth() const
The (AABB) depth.
Definition: kDOP.h:157
hpp::fcl::distance
HPP_FCL_DLLAPI 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.
Definition: kIOS.cpp:181
hpp::fcl::KDOP::volume
FCL_REAL volume() const
The (AABB) volume.
Definition: kDOP.h:160
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::KDOP::dist
FCL_REAL dist(short i) const
Definition: kDOP.h:162
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::KDOP::operator==
bool operator==(const KDOP &other) const
Equality operator.
Definition: kDOP.h:108
hpp::fcl::KDOP::dist
FCL_REAL & dist(short i)
Definition: kDOP.h:164
hpp::fcl::KDOP::height
FCL_REAL height() const
The (AABB) height.
Definition: kDOP.h:154
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: include/hpp/fcl/fwd.hh:57
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::overlap
HPP_FCL_DLLAPI 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.
Definition: AABB.cpp:134
hpp::fcl::KDOP::center
Vec3f center() const
The (AABB) center.
Definition: kDOP.h:146
fwd.hh
hpp::fcl::KDOP::width
FCL_REAL width() const
The (AABB) width.
Definition: kDOP.h:151
hpp::fcl::KDOP::dist_
Eigen::Array< FCL_REAL, N, 1 > dist_
Origin's distances to N KDOP planes.
Definition: kDOP.h:95
hpp::fcl::translate
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
obb.v
list v
Definition: obb.py:48
hpp::fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter,...
Definition: kDOP.h:92
hpp::fcl::KDOP::size
FCL_REAL size() const
Size of the kDOP (used in BV_Splitter to order two kDOPs)
Definition: kDOP.h:141


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14