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 |
|
|
|
36 |
|
|
/** \author Jia Pan */ |
37 |
|
|
|
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 |
|
|
|
46 |
|
117500521 |
AABB::AABB() |
47 |
✓✗ |
117500521 |
: min_(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)())), |
48 |
✓✗✓✗ ✓✗ |
235001042 |
max_(Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)())) {} |
49 |
|
|
|
50 |
|
55997 |
bool AABB::overlap(const AABB& other, const CollisionRequest& request, |
51 |
|
|
FCL_REAL& sqrDistLowerBound) const { |
52 |
|
55997 |
const FCL_REAL break_distance_squared = |
53 |
|
55997 |
request.break_distance * request.break_distance; |
54 |
|
|
|
55 |
|
55997 |
sqrDistLowerBound = |
56 |
✓✗✓✗
|
55997 |
(min_ - other.max_ - Vec3f::Constant(request.security_margin)) |
57 |
✓✗ |
55997 |
.array() |
58 |
✓✗ |
55997 |
.max(FCL_REAL(0)) |
59 |
✓✗ |
55997 |
.matrix() |
60 |
✓✗ |
55997 |
.squaredNorm(); |
61 |
✓✓ |
55997 |
if (sqrDistLowerBound > break_distance_squared) return false; |
62 |
|
|
|
63 |
|
54170 |
sqrDistLowerBound = |
64 |
✓✗✓✗
|
54170 |
(other.min_ - max_ - Vec3f::Constant(request.security_margin)) |
65 |
✓✗ |
54170 |
.array() |
66 |
✓✗ |
54170 |
.max(FCL_REAL(0)) |
67 |
✓✗ |
54170 |
.matrix() |
68 |
✓✗ |
54170 |
.squaredNorm(); |
69 |
✓✓ |
54170 |
if (sqrDistLowerBound > break_distance_squared) return false; |
70 |
|
|
|
71 |
|
51409 |
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 |
|
2689383 |
FCL_REAL AABB::distance(const AABB& other) const { |
115 |
|
2689383 |
FCL_REAL result = 0; |
116 |
✓✓ |
10757532 |
for (Eigen::DenseIndex i = 0; i < 3; ++i) { |
117 |
|
8068149 |
const FCL_REAL& amin = min_[i]; |
118 |
|
8068149 |
const FCL_REAL& amax = max_[i]; |
119 |
|
8068149 |
const FCL_REAL& bmin = other.min_[i]; |
120 |
|
8068149 |
const FCL_REAL& bmax = other.max_[i]; |
121 |
|
|
|
122 |
✓✓ |
8068149 |
if (amin > bmax) { |
123 |
|
3832303 |
FCL_REAL delta = bmax - amin; |
124 |
|
3832303 |
result += delta * delta; |
125 |
✓✓ |
4235846 |
} else if (bmin > amax) { |
126 |
|
3925951 |
FCL_REAL delta = amax - bmin; |
127 |
|
3925951 |
result += delta * delta; |
128 |
|
|
} |
129 |
|
|
} |
130 |
|
|
|
131 |
|
2689383 |
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 |
|
28474 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, |
141 |
|
|
const AABB& b2, const CollisionRequest& request, |
142 |
|
|
FCL_REAL& sqrDistLowerBound) { |
143 |
✓✗✓✗
|
28474 |
AABB bb1(translate(rotate(b1, R0), T0)); |
144 |
✓✗ |
56948 |
return bb1.overlap(b2, request, sqrDistLowerBound); |
145 |
|
|
} |
146 |
|
|
|
147 |
|
|
} // namespace fcl |
148 |
|
|
|
149 |
|
|
} // namespace hpp |