GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/BV/OBB.cpp Lines: 215 243 88.5 %
Date: 2024-02-09 12:57:42 Branches: 477 920 51.8 %

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, Florent Lamiraux */
37
38
#include <hpp/fcl/BV/OBB.h>
39
#include <hpp/fcl/BVH/BVH_utility.h>
40
#include <hpp/fcl/math/transform.h>
41
#include <hpp/fcl/collision_data.h>
42
#include <hpp/fcl/internal/tools.h>
43
44
#include <iostream>
45
#include <limits>
46
47
namespace hpp {
48
namespace fcl {
49
50
/// @brief Compute the 8 vertices of a OBB
51
39222
inline void computeVertices(const OBB& b, Vec3f vertices[8]) {
52

39222
  Matrix3f extAxes(b.axes * b.extent.asDiagonal());
53


39222
  vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1);
54


39222
  vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1);
55


39222
  vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1);
56


39222
  vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1);
57


39222
  vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1);
58


39222
  vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1);
59


39222
  vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1);
60


39222
  vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1);
61
39222
}
62
63
/// @brief OBB merge method when the centers of two smaller OBB are far away
64
21
inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
65
21
  OBB b;
66

357
  Vec3f vertex[16];
67
21
  computeVertices(b1, vertex);
68
21
  computeVertices(b2, vertex + 8);
69
21
  Matrix3f M;
70

84
  Vec3f E[3];
71
21
  Matrix3f::Scalar s[3] = {0, 0, 0};
72
73
  // obb axes
74


21
  b.axes.col(0).noalias() = (b1.To - b2.To).normalized();
75
76

357
  Vec3f vertex_proj[16];
77
357
  for (int i = 0; i < 16; ++i)
78
336
    vertex_proj[i].noalias() =
79



672
        vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
80
81
21
  getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
82
21
  eigen(M, s, E);
83
84
  int min, mid, max;
85
21
  if (s[0] > s[1]) {
86
3
    max = 0;
87
3
    min = 1;
88
  } else {
89
18
    min = 0;
90
18
    max = 1;
91
  }
92
21
  if (s[2] < s[min]) {
93
9
    mid = min;
94
9
    min = 2;
95
12
  } else if (s[2] > s[max]) {
96
6
    mid = max;
97
6
    max = 2;
98
  } else {
99
6
    mid = 2;
100
  }
101
102



21
  b.axes.col(1) << E[0][max], E[1][max], E[2][max];
103



21
  b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid];
104
105
  // set obb centers and extensions
106

21
  Vec3f center, extent;
107
21
  getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent);
108
109

21
  b.To.noalias() = center;
110

21
  b.extent.noalias() = extent;
111
112
42
  return b;
113
}
114
115
/// @brief OBB merge method when the centers of two smaller OBB are close
116
19590
inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
117
19590
  OBB b;
118

19590
  b.To = (b1.To + b2.To) * 0.5;
119

19590
  Quaternion3f q0(b1.axes), q1(b2.axes);
120

19590
  if (q0.dot(q1) < 0) q1.coeffs() *= -1;
121
122

19590
  Quaternion3f q((q0.coeffs() + q1.coeffs()).normalized());
123
19590
  b.axes = q.toRotationMatrix();
124
125

176310
  Vec3f vertex[8], diff;
126
19590
  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
127
19590
  Vec3f pmin(real_max, real_max, real_max);
128
19590
  Vec3f pmax(-real_max, -real_max, -real_max);
129
130
19590
  computeVertices(b1, vertex);
131
176310
  for (int i = 0; i < 8; ++i) {
132

156720
    diff = vertex[i] - b.To;
133
626880
    for (int j = 0; j < 3; ++j) {
134

470160
      FCL_REAL dot = diff.dot(b.axes.col(j));
135

470160
      if (dot > pmax[j])
136
175926
        pmax[j] = dot;
137

294234
      else if (dot < pmin[j])
138
114381
        pmin[j] = dot;
139
    }
140
  }
141
142
19590
  computeVertices(b2, vertex);
143
176310
  for (int i = 0; i < 8; ++i) {
144

156720
    diff = vertex[i] - b.To;
145
626880
    for (int j = 0; j < 3; ++j) {
146

470160
      FCL_REAL dot = diff.dot(b.axes.col(j));
147

470160
      if (dot > pmax[j])
148
55653
        pmax[j] = dot;
149

414507
      else if (dot < pmin[j])
150
52290
        pmin[j] = dot;
151
    }
152
  }
153
154
78360
  for (int j = 0; j < 3; ++j) {
155



58770
    b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j])));
156

58770
    b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
157
  }
158
159
39180
  return b;
160
}
161
162
6000
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
163
                 const Vec3f& b) {
164
  FCL_REAL t, s;
165
6000
  const FCL_REAL reps = 1e-6;
166
167


6000
  Matrix3f Bf(B.array().abs() + reps);
168
  // Bf += reps;
169
170
  // if any of these tests are one-sided, then the polyhedra are disjoint
171
172
  // A1 x A2 = A0
173


6000
  t = ((T[0] < 0.0) ? -T[0] : T[0]);
174
175
  // if(t > (a[0] + Bf.dotX(b)))
176


6000
  if (t > (a[0] + Bf.row(0).dot(b))) return true;
177
178
  // B1 x B2 = B0
179
  // s =  B.transposeDotX(T);
180

5531
  s = B.col(0).dot(T);
181
5531
  t = ((s < 0.0) ? -s : s);
182
183
  // if(t > (b[0] + Bf.transposeDotX(a)))
184


5531
  if (t > (b[0] + Bf.col(0).dot(a))) return true;
185
186
  // A2 x A0 = A1
187


4967
  t = ((T[1] < 0.0) ? -T[1] : T[1]);
188
189
  // if(t > (a[1] + Bf.dotY(b)))
190


4967
  if (t > (a[1] + Bf.row(1).dot(b))) return true;
191
192
  // A0 x A1 = A2
193


3865
  t = ((T[2] < 0.0) ? -T[2] : T[2]);
194
195
  // if(t > (a[2] + Bf.dotZ(b)))
196


3865
  if (t > (a[2] + Bf.row(2).dot(b))) return true;
197
198
  // B2 x B0 = B1
199
  // s = B.transposeDotY(T);
200

3716
  s = B.col(1).dot(T);
201
3716
  t = ((s < 0.0) ? -s : s);
202
203
  // if(t > (b[1] + Bf.transposeDotY(a)))
204


3716
  if (t > (b[1] + Bf.col(1).dot(a))) return true;
205
206
  // B0 x B1 = B2
207
  // s = B.transposeDotZ(T);
208

3612
  s = B.col(2).dot(T);
209
3612
  t = ((s < 0.0) ? -s : s);
210
211
  // if(t > (b[2] + Bf.transposeDotZ(a)))
212


3612
  if (t > (b[2] + Bf.col(2).dot(a))) return true;
213
214
  // A0 x B0
215


3505
  s = T[2] * B(1, 0) - T[1] * B(2, 0);
216
3505
  t = ((s < 0.0) ? -s : s);
217
218
3505
  if (t >
219




3505
      (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
220
21
    return true;
221
222
  // A0 x B1
223


3484
  s = T[2] * B(1, 1) - T[1] * B(2, 1);
224
3484
  t = ((s < 0.0) ? -s : s);
225
226
3484
  if (t >
227




3484
      (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
228
4
    return true;
229
230
  // A0 x B2
231


3480
  s = T[2] * B(1, 2) - T[1] * B(2, 2);
232
3480
  t = ((s < 0.0) ? -s : s);
233
234
3480
  if (t >
235




3480
      (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
236
18
    return true;
237
238
  // A1 x B0
239


3462
  s = T[0] * B(2, 0) - T[2] * B(0, 0);
240
3462
  t = ((s < 0.0) ? -s : s);
241
242
3462
  if (t >
243




3462
      (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
244
8
    return true;
245
246
  // A1 x B1
247


3454
  s = T[0] * B(2, 1) - T[2] * B(0, 1);
248
3454
  t = ((s < 0.0) ? -s : s);
249
250
3454
  if (t >
251




3454
      (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
252
2
    return true;
253
254
  // A1 x B2
255


3452
  s = T[0] * B(2, 2) - T[2] * B(0, 2);
256
3452
  t = ((s < 0.0) ? -s : s);
257
258
3452
  if (t >
259




3452
      (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
260
12
    return true;
261
262
  // A2 x B0
263


3440
  s = T[1] * B(0, 0) - T[0] * B(1, 0);
264
3440
  t = ((s < 0.0) ? -s : s);
265
266
3440
  if (t >
267




3440
      (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
268
30
    return true;
269
270
  // A2 x B1
271


3410
  s = T[1] * B(0, 1) - T[0] * B(1, 1);
272
3410
  t = ((s < 0.0) ? -s : s);
273
274
3410
  if (t >
275




3410
      (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
276
50
    return true;
277
278
  // A2 x B2
279


3360
  s = T[1] * B(0, 2) - T[0] * B(1, 2);
280
3360
  t = ((s < 0.0) ? -s : s);
281
282
3360
  if (t >
283




3360
      (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
284
8
    return true;
285
286
3352
  return false;
287
}
288
289
namespace internal {
290
124361
inline FCL_REAL obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a,
291
                                         const Vec3f& b, const Matrix3f& Bf) {
292
  // |T| - |B| * b - a
293

124361
  Vec3f AABB_corner(T.cwiseAbs() - a);
294

124361
  AABB_corner.noalias() -= Bf * b;
295


124361
  return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm();
296
}
297
298
85075
inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T,
299
                                         const Vec3f& a, const Vec3f& b,
300
                                         const Matrix3f& Bf) {
301
  // Bf = |B|
302
  // | B^T T| - Bf^T * a - b
303
85075
  FCL_REAL s, t = 0;
304


85075
  s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
305
85075
  if (s > 0) t += s * s;
306


85075
  s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
307
85075
  if (s > 0) t += s * s;
308


85075
  s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
309
85075
  if (s > 0) t += s * s;
310
85075
  return t;
311
}
312
313
template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
314
struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi {
315
1203438
  static inline bool run(int ia, int ja, int ka, const Matrix3f& B,
316
                         const Vec3f& T, const Vec3f& a, const Vec3f& b,
317
                         const Matrix3f& Bf, const FCL_REAL& breakDistance2,
318
                         FCL_REAL& squaredLowerBoundDistance) {
319
1203438
    FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
320
1203438
    if (sinus2 < 1e-6) return false;
321
322
1200238
    const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
323
324
1200238
    const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
325
1200238
                                     b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
326
    // We need to divide by the norm || Aia x Bib ||
327
    // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2  = cosine^2
328
1200238
    if (diff > 0) {
329
10080
      squaredLowerBoundDistance = diff * diff / sinus2;
330
10080
      if (squaredLowerBoundDistance > breakDistance2) {
331
10080
        return true;
332
      }
333
    }
334
1190158
    return false;
335
  }
336
};
337
}  // namespace internal
338
339
// B, T orientation and position of 2nd OBB in frame of 1st OBB,
340
// a extent of 1st OBB,
341
// b extent of 2nd OBB.
342
//
343
// This function tests whether bounding boxes should be broken down.
344
//
345
124361
bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T,
346
                                      const Vec3f& a_, const Vec3f& b_,
347
                                      const CollisionRequest& request,
348
                                      FCL_REAL& squaredLowerBoundDistance) {
349

124361
  assert(request.security_margin >
350
             -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) -
351
                 10 * Eigen::NumTraits<FCL_REAL>::epsilon() &&
352
         "A negative security margin could not be lower than the OBB extent.");
353
  //  const FCL_REAL breakDistance(request.break_distance +
354
  //                               request.security_margin);
355
124361
  const FCL_REAL breakDistance2 =
356
124361
      request.break_distance * request.break_distance;
357
358

124361
  Matrix3f Bf(B.cwiseAbs());
359

124361
  const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2))
360
124361
                    .array()
361

124361
                    .max(FCL_REAL(0)));
362

124361
  const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2))
363
124361
                    .array()
364

124361
                    .max(FCL_REAL(0)));
365
366
  // Corner of b axis aligned bounding box the closest to the origin
367
124361
  squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf);
368
124361
  if (squaredLowerBoundDistance > breakDistance2) return true;
369
370
85075
  squaredLowerBoundDistance =
371
85075
      internal::obbDisjoint_check_B_axis(B, T, a, b, Bf);
372
85075
  if (squaredLowerBoundDistance > breakDistance2) return true;
373
374
  // Ai x Bj
375
70607
  int ja = 1, ka = 2;
376
269053
  for (int ia = 0; ia < 3; ++ia) {
377

203486
    if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run(
378
            ia, ja, ka, B, T, a, b, Bf, breakDistance2,
379
            squaredLowerBoundDistance))
380
3928
      return true;
381

199558
    if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run(
382
            ia, ja, ka, B, T, a, b, Bf, breakDistance2,
383
            squaredLowerBoundDistance))
384
883
      return true;
385

198675
    if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run(
386
            ia, ja, ka, B, T, a, b, Bf, breakDistance2,
387
            squaredLowerBoundDistance))
388
229
      return true;
389
198446
    ja = ka;
390
198446
    ka = ia;
391
  }
392
393
65567
  return false;
394
}
395
396
6000
bool OBB::overlap(const OBB& other) const {
397
  /// compute what transform [R,T] that takes us from cs1 to cs2.
398
  /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
399
  /// First compute the rotation part, then translation part
400


6000
  Vec3f T(axes.transpose() * (other.To - To));
401

6000
  Matrix3f R(axes.transpose() * other.axes);
402
403
6000
  return !obbDisjoint(R, T, extent, other.extent);
404
}
405
406
69552
bool OBB::overlap(const OBB& other, const CollisionRequest& request,
407
                  FCL_REAL& sqrDistLowerBound) const {
408
  /// compute what transform [R,T] that takes us from cs1 to cs2.
409
  /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
410
  /// First compute the rotation part, then translation part
411
  // Vec3f t = other.To - To; // T2 - T1
412
  // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
413
  // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
414
  // axis[0].dot(other.axis[2]),
415
  // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
416
  // axis[1].dot(other.axis[2]),
417
  // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
418
  // axis[2].dot(other.axis[2]));
419


69552
  Vec3f T(axes.transpose() * (other.To - To));
420

69552
  Matrix3f R(axes.transpose() * other.axes);
421
422
69552
  return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request,
423
69552
                                           sqrDistLowerBound);
424
}
425
426
bool OBB::contain(const Vec3f& p) const {
427
  Vec3f local_p(p - To);
428
  FCL_REAL proj = local_p.dot(axes.col(0));
429
  if ((proj > extent[0]) || (proj < -extent[0])) return false;
430
431
  proj = local_p.dot(axes.col(1));
432
  if ((proj > extent[1]) || (proj < -extent[1])) return false;
433
434
  proj = local_p.dot(axes.col(2));
435
  if ((proj > extent[2]) || (proj < -extent[2])) return false;
436
437
  return true;
438
}
439
440
OBB& OBB::operator+=(const Vec3f& p) {
441
  OBB bvp;
442
  bvp.To = p;
443
  bvp.axes.noalias() = axes;
444
  bvp.extent.setZero();
445
446
  *this += bvp;
447
  return *this;
448
}
449
450
19611
OBB OBB::operator+(const OBB& other) const {
451

19611
  Vec3f center_diff = To - other.To;
452

19611
  FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
453
  FCL_REAL max_extent2 =
454

19611
      std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
455

19611
  if (center_diff.norm() > 2 * (max_extent + max_extent2)) {
456
21
    return merge_largedist(*this, other);
457
  } else {
458
19590
    return merge_smalldist(*this, other);
459
  }
460
}
461
462
FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const {
463
  std::cerr << "OBB distance not implemented!" << std::endl;
464
  return 0.0;
465
}
466
467
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
468
             const OBB& b2) {
469
  Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
470
  Vec3f T(b1.axes.transpose() * Ttemp);
471
  Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
472
473
  return !obbDisjoint(R, T, b1.extent, b2.extent);
474
}
475
476
54809
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
477
             const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) {
478


54809
  Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
479

54809
  Vec3f T(b1.axes.transpose() * Ttemp);
480


54809
  Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
481
482
54809
  return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request,
483
54809
                                           sqrDistLowerBound);
484
}
485
486
OBB translate(const OBB& bv, const Vec3f& t) {
487
  OBB res(bv);
488
  res.To += t;
489
  return res;
490
}
491
492
}  // namespace fcl
493
494
}  // namespace hpp