hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
44namespace hpp {
45namespace fcl {
46
47struct CollisionRequest;
48
51
91template <short N>
93 protected:
95 Eigen::Array<FCL_REAL, N, 1> dist_;
96
97 public:
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
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
175template <short N>
176bool 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
181template <short N>
182bool 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
189template <short N>
191
192} // namespace fcl
193
194} // namespace hpp
195
196#endif
KDOP class describes the KDOP collision structures. K is set as the template parameter,...
Definition: kDOP.h:92
#define HPP_FCL_DLLAPI
Definition: config.hh:88
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
FCL_REAL distance(const KDOP< N > &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
The distance between two KDOP<N>. Not implemented.
KDOP(const Vec3f &v)
Creating kDOP containing only one point.
FCL_REAL size() const
Size of the kDOP (used in BV_Splitter to order two kDOPs)
Definition: kDOP.h:141
KDOP< N > operator+(const KDOP< N > &other) const
Create a KDOP by mergin two KDOPs.
FCL_REAL & dist(short i)
Definition: kDOP.h:164
KDOP< N > & operator+=(const KDOP< N > &other)
Merge two KDOPs.
bool operator==(const KDOP &other) const
Equality operator.
Definition: kDOP.h:108
Eigen::Array< FCL_REAL, N, 1 > dist_
Origin's distances to N KDOP planes.
Definition: kDOP.h:95
FCL_REAL volume() const
The (AABB) volume.
Definition: kDOP.h:160
KDOP< N > & operator+=(const Vec3f &p)
Merge the point and the KDOP.
FCL_REAL depth() const
The (AABB) depth.
Definition: kDOP.h:157
KDOP()
Creating kDOP containing nothing.
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 height() const
The (AABB) height.
Definition: kDOP.h:154
bool overlap(const KDOP< N > &other) const
Check whether two KDOPs overlap.
bool operator!=(const KDOP &other) const
Difference operator.
Definition: kDOP.h:113
bool overlap(const KDOP< N > &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two KDOPs overlap.
bool inside(const Vec3f &p) const
FCL_REAL width() const
The (AABB) width.
Definition: kDOP.h:151
KDOP(const Vec3f &a, const Vec3f &b)
Creating kDOP containing two points.
FCL_REAL dist(short i) const
Definition: kDOP.h:162
Vec3f center() const
The (AABB) center.
Definition: kDOP.h:146
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