GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/hpp/fcl/BV/BV.h Lines: 57 83 68.7 %
Date: 2024-02-09 12:57:42 Branches: 44 162 27.2 %

Line Branch Exec Source
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
#ifndef HPP_FCL_BV_H
39
#define HPP_FCL_BV_H
40
41
#include <hpp/fcl/BV/kDOP.h>
42
#include <hpp/fcl/BV/AABB.h>
43
#include <hpp/fcl/BV/OBB.h>
44
#include <hpp/fcl/BV/RSS.h>
45
#include <hpp/fcl/BV/OBBRSS.h>
46
#include <hpp/fcl/BV/kIOS.h>
47
#include <hpp/fcl/math/transform.h>
48
49
/** @brief Main namespace */
50
namespace hpp {
51
namespace fcl {
52
53
/// @cond IGNORE
54
namespace details {
55
56
/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a
57
/// bounding volume of type BV2 in I configuration.
58
template <typename BV1, typename BV2>
59
struct Converter {
60
  static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2);
61
  static void convert(const BV1& bv1, BV2& bv2);
62
};
63
64
/// @brief Convert from AABB to AABB, not very tight but is fast.
65
template <>
66
struct Converter<AABB, AABB> {
67
  static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) {
68
    const Vec3f& center = bv1.center();
69
    FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
70
    const Vec3f center2 = tf1.transform(center);
71
    bv2.min_ = center2 - Vec3f::Constant(r);
72
    bv2.max_ = center2 + Vec3f::Constant(r);
73
  }
74
75
117720
  static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
76
};
77
78
template <>
79
struct Converter<AABB, OBB> {
80
30985
  static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) {
81
30985
    bv2.To = tf1.transform(bv1.center());
82


30985
    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
83
30985
    bv2.axes = tf1.getRotation();
84
30985
  }
85
86
98318
  static void convert(const AABB& bv1, OBB& bv2) {
87
98318
    bv2.To = bv1.center();
88


98318
    bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
89
98318
    bv2.axes.setIdentity();
90
98318
  }
91
};
92
93
template <>
94
struct Converter<OBB, OBB> {
95
28982
  static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) {
96
28982
    bv2.extent = bv1.extent;
97
28982
    bv2.To = tf1.transform(bv1.To);
98

28982
    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
99
28982
  }
100
101
  static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
102
};
103
104
template <>
105
struct Converter<OBBRSS, OBB> {
106
28982
  static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) {
107
28982
    Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
108
28982
  }
109
110
  static void convert(const OBBRSS& bv1, OBB& bv2) {
111
    Converter<OBB, OBB>::convert(bv1.obb, bv2);
112
  }
113
};
114
115
template <>
116
struct Converter<RSS, OBB> {
117
  static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) {
118
    bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
119
                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
120
    bv2.To = tf1.transform(bv1.Tr);
121
    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
122
  }
123
124
  static void convert(const RSS& bv1, OBB& bv2) {
125
    bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
126
                       bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
127
    bv2.To = bv1.Tr;
128
    bv2.axes = bv1.axes;
129
  }
130
};
131
132
template <typename BV1>
133
struct Converter<BV1, AABB> {
134
  static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) {
135
    const Vec3f& center = bv1.center();
136
    FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
137
    const Vec3f center2 = tf1.transform(center);
138
    bv2.min_ = center2 - Vec3f::Constant(r);
139
    bv2.max_ = center2 + Vec3f::Constant(r);
140
  }
141
142
  static void convert(const BV1& bv1, AABB& bv2) {
143
    const Vec3f& center = bv1.center();
144
    FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
145
    bv2.min_ = center - Vec3f::Constant(r);
146
    bv2.max_ = center + Vec3f::Constant(r);
147
  }
148
};
149
150
template <typename BV1>
151
struct Converter<BV1, OBB> {
152
  static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) {
153
    AABB bv;
154
    Converter<BV1, AABB>::convert(bv1, bv);
155
    Converter<AABB, OBB>::convert(bv, tf1, bv2);
156
  }
157
158
  static void convert(const BV1& bv1, OBB& bv2) {
159
    AABB bv;
160
    Converter<BV1, AABB>::convert(bv1, bv);
161
    Converter<AABB, OBB>::convert(bv, bv2);
162
  }
163
};
164
165
template <>
166
struct Converter<OBB, RSS> {
167
  static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) {
168
    bv2.Tr = tf1.transform(bv1.To);
169
    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
170
171
    bv2.radius = bv1.extent[2];
172
    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
173
    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
174
  }
175
176
  static void convert(const OBB& bv1, RSS& bv2) {
177
    bv2.Tr = bv1.To;
178
    bv2.axes = bv1.axes;
179
180
    bv2.radius = bv1.extent[2];
181
    bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
182
    bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
183
  }
184
};
185
186
template <>
187
struct Converter<RSS, RSS> {
188
  static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) {
189
    bv2.Tr = tf1.transform(bv1.Tr);
190
    bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
191
192
    bv2.radius = bv1.radius;
193
    bv2.length[0] = bv1.length[0];
194
    bv2.length[1] = bv1.length[1];
195
  }
196
197
  static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
198
};
199
200
template <>
201
struct Converter<OBBRSS, RSS> {
202
  static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) {
203
    Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
204
  }
205
206
  static void convert(const OBBRSS& bv1, RSS& bv2) {
207
    Converter<RSS, RSS>::convert(bv1.rss, bv2);
208
  }
209
};
210
211
template <>
212
struct Converter<AABB, RSS> {
213
98318
  static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) {
214

98318
    bv2.Tr = tf1.transform(bv1.center());
215
216
    /// Sort the AABB edges so that AABB extents are ordered.
217

98318
    FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth()};
218
98318
    Eigen::DenseIndex id[3] = {0, 1, 2};
219
220
294954
    for (Eigen::DenseIndex i = 1; i < 3; ++i) {
221
491590
      for (Eigen::DenseIndex j = i; j > 0; --j) {
222
294954
        if (d[j] > d[j - 1]) {
223
          {
224
265551
            FCL_REAL tmp = d[j];
225
265551
            d[j] = d[j - 1];
226
265551
            d[j - 1] = tmp;
227
          }
228
          {
229
265551
            Eigen::DenseIndex tmp = id[j];
230
265551
            id[j] = id[j - 1];
231
265551
            id[j - 1] = tmp;
232
          }
233
        }
234
      }
235
    }
236
237

98318
    const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
238
98318
    bv2.radius = extent[id[2]];
239
98318
    bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
240
98318
    bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
241
242
98318
    const Matrix3f& R = tf1.getRotation();
243
98318
    const bool left_hand = (id[0] == (id[1] + 1) % 3);
244
98318
    if (left_hand)
245


68941
      bv2.axes.col(0) = -R.col(id[0]);
246
    else
247

29377
      bv2.axes.col(0) = R.col(id[0]);
248

98318
    bv2.axes.col(1) = R.col(id[1]);
249

98318
    bv2.axes.col(2) = R.col(id[2]);
250
98318
  }
251
252
98318
  static void convert(const AABB& bv1, RSS& bv2) {
253
98318
    convert(bv1, Transform3f(), bv2);
254
98318
  }
255
};
256
257
template <>
258
struct Converter<AABB, OBBRSS> {
259
  static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) {
260
    Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
261
    Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
262
  }
263
264
98318
  static void convert(const AABB& bv1, OBBRSS& bv2) {
265
98318
    Converter<AABB, OBB>::convert(bv1, bv2.obb);
266
98318
    Converter<AABB, RSS>::convert(bv1, bv2.rss);
267
98318
  }
268
};
269
270
}  // namespace details
271
272
/// @endcond
273
274
/// @brief Convert a bounding volume of type BV1 in configuration tf1 to
275
/// bounding volume of type BV2 in identity configuration.
276
template <typename BV1, typename BV2>
277
117931
static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) {
278
117931
  details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
279
117931
}
280
281
/// @brief Convert a bounding volume of type BV1 to bounding volume of type BV2
282
/// in identity configuration.
283
template <typename BV1, typename BV2>
284
392675
static inline void convertBV(const BV1& bv1, BV2& bv2) {
285
392675
  details::Converter<BV1, BV2>::convert(bv1, bv2);
286
392675
}
287
288
}  // namespace fcl
289
290
}  // namespace hpp
291
292
#endif