AABB.cpp
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 #include "coal/BV/AABB.h"
40 #include "coal/collision_data.h"
41 
42 #include <limits>
43 
44 namespace coal {
45 
47  : min_(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
48  max_(Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)())) {}
49 
50 bool AABB::overlap(const AABB& other, const CollisionRequest& request,
51  CoalScalar& sqrDistLowerBound) const {
52  const CoalScalar break_distance_squared =
53  request.break_distance * request.break_distance;
54 
55  sqrDistLowerBound =
56  (min_ - other.max_ - Vec3s::Constant(request.security_margin))
57  .array()
58  .max(CoalScalar(0))
59  .matrix()
60  .squaredNorm();
61  if (sqrDistLowerBound > break_distance_squared) return false;
62 
63  sqrDistLowerBound =
64  (other.min_ - max_ - Vec3s::Constant(request.security_margin))
65  .array()
66  .max(CoalScalar(0))
67  .matrix()
68  .squaredNorm();
69  if (sqrDistLowerBound > break_distance_squared) return false;
70 
71  return true;
72 }
73 
74 CoalScalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const {
75  CoalScalar result = 0;
76  for (Eigen::DenseIndex i = 0; i < 3; ++i) {
77  const CoalScalar& amin = min_[i];
78  const CoalScalar& amax = max_[i];
79  const CoalScalar& bmin = other.min_[i];
80  const CoalScalar& bmax = other.max_[i];
81 
82  if (amin > bmax) {
83  CoalScalar delta = bmax - amin;
84  result += delta * delta;
85  if (P && Q) {
86  (*P)[i] = amin;
87  (*Q)[i] = bmax;
88  }
89  } else if (bmin > amax) {
90  CoalScalar delta = amax - bmin;
91  result += delta * delta;
92  if (P && Q) {
93  (*P)[i] = amax;
94  (*Q)[i] = bmin;
95  }
96  } else {
97  if (P && Q) {
98  if (bmin >= amin) {
99  CoalScalar t = 0.5 * (amax + bmin);
100  (*P)[i] = t;
101  (*Q)[i] = t;
102  } else {
103  CoalScalar t = 0.5 * (amin + bmax);
104  (*P)[i] = t;
105  (*Q)[i] = t;
106  }
107  }
108  }
109  }
110 
111  return std::sqrt(result);
112 }
113 
114 CoalScalar AABB::distance(const AABB& other) const {
115  CoalScalar result = 0;
116  for (Eigen::DenseIndex i = 0; i < 3; ++i) {
117  const CoalScalar& amin = min_[i];
118  const CoalScalar& amax = max_[i];
119  const CoalScalar& bmin = other.min_[i];
120  const CoalScalar& bmax = other.max_[i];
121 
122  if (amin > bmax) {
123  CoalScalar delta = bmax - amin;
124  result += delta * delta;
125  } else if (bmin > amax) {
126  CoalScalar delta = amax - bmin;
127  result += delta * delta;
128  }
129  }
130 
131  return std::sqrt(result);
132 }
133 
134 bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
135  const AABB& b2) {
136  AABB bb1(translate(rotate(b1, R0), T0));
137  return bb1.overlap(b2);
138 }
139 
140 bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
141  const AABB& b2, const CollisionRequest& request,
142  CoalScalar& sqrDistLowerBound) {
143  AABB bb1(translate(rotate(b1, R0), T0));
144  return bb1.overlap(b2, request, sqrDistLowerBound);
145 }
146 
147 bool AABB::overlap(const Plane& p) const {
148  // Convert AABB to a (box, transform) representation and compute the support
149  // points in the directions normal and -normal.
150  // If both points lie on different sides of the plane, there is an overlap
151  // between the AABB and the plane. Otherwise, there is no overlap.
152  const Vec3s halfside = (this->max_ - this->min_) / 2;
153  const Vec3s center = (this->max_ + this->min_) / 2;
154 
155  const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
156  const Vec3s support2 =
157  ((-p.n).array() > 0).select(halfside, -halfside) + center;
158 
159  const CoalScalar dist1 = p.n.dot(support1) - p.d;
160  const CoalScalar dist2 = p.n.dot(support2) - p.d;
161  const int sign1 = (dist1 > 0) ? 1 : -1;
162  const int sign2 = (dist2 > 0) ? 1 : -1;
163 
164  if (p.getSweptSphereRadius() > 0) {
165  if (sign1 != sign2) {
166  // Supports are on different sides of the plane. There is an overlap.
167  return true;
168  }
169  // Both supports are on the same side of the plane.
170  // We now need to check if they are on the same side of the plane inflated
171  // by the swept-sphere radius.
172  const CoalScalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius();
173  const CoalScalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius();
174  const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1;
175  const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1;
176  return ssr_sign1 != ssr_sign2;
177  }
178 
179  return (sign1 != sign2);
180 }
181 
182 bool AABB::overlap(const Halfspace& hs) const {
183  // Convert AABB to a (box, transform) representation and compute the support
184  // points in the direction -normal.
185  // If the support is below the plane defined by the halfspace, there is an
186  // overlap between the AABB and the halfspace. Otherwise, there is no
187  // overlap.
188  Vec3s halfside = (this->max_ - this->min_) / 2;
189  Vec3s center = (this->max_ + this->min_) / 2;
190  Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
191  return (hs.signedDistance(support) < 0);
192 }
193 
194 } // namespace coal
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::AABB::distance
CoalScalar distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
coal::Halfspace
Half Space: this is equivalent to the Plane in ODE. A Half space has a priviledged direction: the dir...
Definition: coal/shape/geometric_shapes.h:892
coal::AABB::AABB
AABB()
Creating an AABB with zero size (low bound +inf, upper bound -inf)
Definition: AABB.cpp:46
coal::CollisionRequest::security_margin
CoalScalar security_margin
Distance below which objects are considered in collision. See Collision.
Definition: coal/collision_data.h:328
collision_data.h
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
coal::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: coal/BV/AABB.h:55
coal::Plane
Infinite plane. A plane can be viewed as two half spaces; it has no priviledged direction....
Definition: coal/shape/geometric_shapes.h:983
coal::AABB::max_
Vec3s max_
The max point in the AABB.
Definition: coal/BV/AABB.h:60
coal::AABB::overlap
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: coal/BV/AABB.h:111
coal::detail::select
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2
Definition: coal/broadphase/detail/hierarchy_tree-inl.h:952
P
P
coal::CollisionRequest
request to the collision algorithm
Definition: coal/collision_data.h:311
coal::Halfspace::signedDistance
CoalScalar signedDistance(const Vec3s &p) const
Definition: coal/shape/geometric_shapes.h:920
coal::translate
static AABB translate(const AABB &aabb, const Vec3s &t)
translate the center of AABB by t
Definition: coal/BV/AABB.h:233
coal::AABB::center
Vec3s center() const
Center of the AABB.
Definition: coal/BV/AABB.h:164
coal::overlap
COAL_DLLAPI bool overlap(const Matrix3s &R0, const Vec3s &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
coal::AABB::min_
Vec3s min_
The min point in the AABB.
Definition: coal/BV/AABB.h:58
coal::ShapeBase::getSweptSphereRadius
CoalScalar getSweptSphereRadius() const
Get radius of sphere swept around the shape. This radius is always >= 0.
Definition: coal/shape/geometric_shapes.h:86
coal::rotate
static AABB rotate(const AABB &aabb, const Matrix3s &R)
Definition: coal/BV/AABB.h:240
coal::Halfspace::n
Vec3s n
Plane normal.
Definition: coal/shape/geometric_shapes.h:956
coal::Matrix3s
Eigen::Matrix< CoalScalar, 3, 3 > Matrix3s
Definition: coal/data_types.h:81
coal::Plane::n
Vec3s n
Plane normal.
Definition: coal/shape/geometric_shapes.h:1034
geometric_shapes.h
t
dictionary t
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
coal::CollisionRequest::break_distance
CoalScalar break_distance
Distance below which bounding volumes are broken down. See Collision.
Definition: coal/collision_data.h:332
coal::Plane::d
CoalScalar d
Plane offset.
Definition: coal/shape/geometric_shapes.h:1037
AABB.h


hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:57