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 <hpp/fcl/BV/AABB.h>
39 
40 #include <limits>
41 #include <hpp/fcl/collision_data.h>
42 
43 namespace hpp {
44 namespace fcl {
45 
47  : min_(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)())),
48  max_(Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)())) {}
49 
50 bool AABB::overlap(const AABB& other, const CollisionRequest& request,
51  FCL_REAL& sqrDistLowerBound) const {
52  const FCL_REAL break_distance_squared =
53  request.break_distance * request.break_distance;
54 
55  sqrDistLowerBound =
56  (min_ - other.max_ - Vec3f::Constant(request.security_margin))
57  .array()
58  .max(0)
59  .matrix()
60  .squaredNorm();
61  if (sqrDistLowerBound > break_distance_squared) return false;
62 
63  sqrDistLowerBound =
64  (other.min_ - max_ - Vec3f::Constant(request.security_margin))
65  .array()
66  .max(0)
67  .matrix()
68  .squaredNorm();
69  if (sqrDistLowerBound > break_distance_squared) return false;
70 
71  return true;
72 }
73 
74 FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const {
75  FCL_REAL result = 0;
76  for (Eigen::DenseIndex i = 0; i < 3; ++i) {
77  const FCL_REAL& amin = min_[i];
78  const FCL_REAL& amax = max_[i];
79  const FCL_REAL& bmin = other.min_[i];
80  const FCL_REAL& bmax = other.max_[i];
81 
82  if (amin > bmax) {
83  FCL_REAL 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  FCL_REAL 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  FCL_REAL t = 0.5 * (amax + bmin);
100  (*P)[i] = t;
101  (*Q)[i] = t;
102  } else {
103  FCL_REAL 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 FCL_REAL AABB::distance(const AABB& other) const {
115  FCL_REAL result = 0;
116  for (Eigen::DenseIndex i = 0; i < 3; ++i) {
117  const FCL_REAL& amin = min_[i];
118  const FCL_REAL& amax = max_[i];
119  const FCL_REAL& bmin = other.min_[i];
120  const FCL_REAL& bmax = other.max_[i];
121 
122  if (amin > bmax) {
123  FCL_REAL delta = bmax - amin;
124  result += delta * delta;
125  } else if (bmin > amax) {
126  FCL_REAL delta = amax - bmin;
127  result += delta * delta;
128  }
129  }
130 
131  return std::sqrt(result);
132 }
133 
134 bool overlap(const Matrix3f& R0, const Vec3f& 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 Matrix3f& R0, const Vec3f& T0, const AABB& b1,
141  const AABB& b2, const CollisionRequest& request,
142  FCL_REAL& sqrDistLowerBound) {
143  AABB bb1(translate(rotate(b1, R0), T0));
144  return bb1.overlap(b2, request, sqrDistLowerBound);
145 }
146 
147 } // namespace fcl
148 
149 } // namespace hpp
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: BV/AABB.h:110
Main namespace.
t
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
static AABB rotate(const AABB &aabb, const Matrix3f &R)
Definition: BV/AABB.h:233
AABB()
Creating an AABB with zero size (low bound +inf, upper bound -inf)
Definition: AABB.cpp:46
request to the collision algorithm
double FCL_REAL
Definition: data_types.h:65
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision.
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00