GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/BVH/BVH_utility.cpp Lines: 324 379 85.5 %
Date: 2024-02-09 12:57:42 Branches: 393 904 43.5 %

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
#include <hpp/fcl/BVH/BVH_utility.h>
39
#include <hpp/fcl/narrowphase/narrowphase.h>
40
#include <hpp/fcl/shape/geometric_shapes_utility.h>
41
42
namespace hpp {
43
namespace fcl {
44
45
namespace details {
46
template <typename BV>
47
80
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
48
                         const AABB& _aabb) {
49
80
  assert(model.getModelType() == BVH_MODEL_TRIANGLES);
50
80
  const Matrix3f& q = pose.getRotation();
51

80
  AABB aabb = translate(_aabb, -pose.getTranslation());
52
53
80
  Transform3f box_pose;
54
160
  Box box;
55
80
  constructBox(_aabb, box, box_pose);
56

80
  box_pose = pose.inverseTimes(box_pose);
57
58
80
  GJKSolver gjk;
59
60
  // Check what triangles should be kept.
61
  // TODO use the BV hierarchy
62
160
  std::vector<bool> keep_vertex(model.num_vertices, false);
63
160
  std::vector<bool> keep_tri(model.num_tris, false);
64
80
  unsigned int ntri = 0;
65
1040
  for (unsigned int i = 0; i < model.num_tris; ++i) {
66
960
    const Triangle& t = model.tri_indices[i];
67
68
960
    bool keep_this_tri =
69

960
        keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]];
70
71
960
    if (!keep_this_tri) {
72
2720
      for (unsigned int j = 0; j < 3; ++j) {
73


2240
        if (aabb.contain(q * model.vertices[t[j]])) {
74
480
          keep_this_tri = true;
75
480
          break;
76
        }
77
      }
78
960
      const Vec3f& p0 = model.vertices[t[0]];
79
960
      const Vec3f& p1 = model.vertices[t[1]];
80
960
      const Vec3f& p2 = model.vertices[t[2]];
81

960
      Vec3f c1, c2, normal;
82
      FCL_REAL distance;
83
1440
      if (!keep_this_tri &&
84


1440
          gjk.shapeTriangleInteraction(box, box_pose, p0, p1, p2, Transform3f(),
85
                                       distance, c1, c2, normal)) {
86
32
        keep_this_tri = true;
87
      }
88
    }
89
960
    if (keep_this_tri) {
90
512
      keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true;
91
512
      keep_tri[i] = true;
92
512
      ntri++;
93
    }
94
  }
95
96
80
  if (ntri == 0) return NULL;
97
98

64
  BVHModel<BV>* new_model(new BVHModel<BV>());
99
64
  new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices));
100
128
  std::vector<unsigned int> idxConversion(model.num_vertices);
101
64
  assert(new_model->num_vertices == 0);
102
2368
  for (unsigned int i = 0; i < keep_vertex.size(); ++i) {
103
2304
    if (keep_vertex[i]) {
104
1536
      idxConversion[i] = new_model->num_vertices;
105
1536
      new_model->vertices[new_model->num_vertices] = model.vertices[i];
106
1536
      new_model->num_vertices++;
107
    }
108
  }
109
64
  assert(new_model->num_tris == 0);
110
832
  for (unsigned int i = 0; i < keep_tri.size(); ++i) {
111
768
    if (keep_tri[i]) {
112
512
      new_model->tri_indices[new_model->num_tris].set(
113
512
          idxConversion[model.tri_indices[i][0]],
114
512
          idxConversion[model.tri_indices[i][1]],
115
512
          idxConversion[model.tri_indices[i][2]]);
116
512
      new_model->num_tris++;
117
    }
118
  }
119

64
  if (new_model->endModel() != BVH_OK) {
120
    delete new_model;
121
    return NULL;
122
  }
123
64
  return new_model;
124
}
125
}  // namespace details
126
127
template <>
128
5
BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model, const Transform3f& pose,
129
                          const AABB& aabb) {
130
5
  return details::BVHExtract(model, pose, aabb);
131
}
132
template <>
133
5
BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model, const Transform3f& pose,
134
                           const AABB& aabb) {
135
5
  return details::BVHExtract(model, pose, aabb);
136
}
137
template <>
138
5
BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model, const Transform3f& pose,
139
                          const AABB& aabb) {
140
5
  return details::BVHExtract(model, pose, aabb);
141
}
142
template <>
143
5
BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model, const Transform3f& pose,
144
                           const AABB& aabb) {
145
5
  return details::BVHExtract(model, pose, aabb);
146
}
147
template <>
148
5
BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model,
149
                             const Transform3f& pose, const AABB& aabb) {
150
5
  return details::BVHExtract(model, pose, aabb);
151
}
152
template <>
153
5
BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model,
154
                                const Transform3f& pose, const AABB& aabb) {
155
5
  return details::BVHExtract(model, pose, aabb);
156
}
157
template <>
158
5
BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model,
159
                                const Transform3f& pose, const AABB& aabb) {
160
5
  return details::BVHExtract(model, pose, aabb);
161
}
162
template <>
163
5
BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
164
                                const Transform3f& pose, const AABB& aabb) {
165
5
  return details::BVHExtract(model, pose, aabb);
166
}
167
168
1777280
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
169
                   unsigned int n, Matrix3f& M) {
170

1777280
  Vec3f S1(Vec3f::Zero());
171



1777280
  Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()};
172
173
1777280
  if (ts) {
174
11844083
    for (unsigned int i = 0; i < n; ++i) {
175
10080352
      const Triangle& t = (indices) ? ts[indices[i]] : ts[i];
176
177
10080352
      const Vec3f& p1 = ps[t[0]];
178
10080352
      const Vec3f& p2 = ps[t[1]];
179
10080352
      const Vec3f& p3 = ps[t[2]];
180
181


10080352
      S1[0] += (p1[0] + p2[0] + p3[0]);
182


10080352
      S1[1] += (p1[1] + p2[1] + p3[1]);
183


10080352
      S1[2] += (p1[2] + p2[2] + p3[2]);
184



10080352
      S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
185



10080352
      S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
186



10080352
      S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
187



10080352
      S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
188



10080352
      S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
189



10080352
      S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
190
191
10080352
      if (ps2) {
192
        const Vec3f& p1 = ps2[t[0]];
193
        const Vec3f& p2 = ps2[t[1]];
194
        const Vec3f& p3 = ps2[t[2]];
195
196
        S1[0] += (p1[0] + p2[0] + p3[0]);
197
        S1[1] += (p1[1] + p2[1] + p3[1]);
198
        S1[2] += (p1[2] + p2[2] + p3[2]);
199
200
        S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
201
        S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
202
        S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
203
        S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
204
        S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
205
        S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
206
      }
207
    }
208
  } else {
209
226977
    for (unsigned int i = 0; i < n; ++i) {
210
213428
      const Vec3f& p = (indices) ? ps[indices[i]] : ps[i];
211
213428
      S1 += p;
212

213428
      S2[0][0] += (p[0] * p[0]);
213

213428
      S2[1][1] += (p[1] * p[1]);
214

213428
      S2[2][2] += (p[2] * p[2]);
215

213428
      S2[0][1] += (p[0] * p[1]);
216

213428
      S2[0][2] += (p[0] * p[2]);
217

213428
      S2[1][2] += (p[1] * p[2]);
218
219
213428
      if (ps2)  // another frame
220
      {
221
        const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i];
222
        S1 += p;
223
        S2[0][0] += (p[0] * p[0]);
224
        S2[1][1] += (p[1] * p[1]);
225
        S2[2][2] += (p[2] * p[2]);
226
        S2[0][1] += (p[0] * p[1]);
227
        S2[0][2] += (p[0] * p[2]);
228
        S2[1][2] += (p[1] * p[2]);
229
      }
230
    }
231
  }
232
233

1777280
  unsigned int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
234
235


1777280
  M(0, 0) = S2[0][0] - S1[0] * S1[0] / n_points;
236


1777280
  M(1, 1) = S2[1][1] - S1[1] * S1[1] / n_points;
237


1777280
  M(2, 2) = S2[2][2] - S1[2] * S1[2] / n_points;
238


1777280
  M(0, 1) = S2[0][1] - S1[0] * S1[1] / n_points;
239


1777280
  M(1, 2) = S2[1][2] - S1[1] * S1[2] / n_points;
240


1777280
  M(0, 2) = S2[0][2] - S1[0] * S1[2] / n_points;
241

1777280
  M(1, 0) = M(0, 1);
242

1777280
  M(2, 0) = M(0, 2);
243

1777280
  M(2, 1) = M(1, 2);
244
1777280
}
245
246
/** @brief Compute the RSS bounding volume parameters: radius, rectangle size
247
 * and the origin. The bounding volume axes are known.
248
 */
249
1335766
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
250
                                        unsigned int* indices, unsigned int n,
251
                                        const Matrix3f& axes, Vec3f& origin,
252
                                        FCL_REAL l[2], FCL_REAL& r) {
253
1335766
  bool indirect_index = true;
254
1335766
  if (!indices) indirect_index = false;
255
256

1335766
  unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
257
258

1335766
  FCL_REAL(*P)[3] = new FCL_REAL[size_P][3];
259
260
1335766
  int P_id = 0;
261
262
1335766
  if (ts) {
263
8501055
    for (unsigned int i = 0; i < n; ++i) {
264
7191670
      unsigned int index = indirect_index ? indices[i] : i;
265
7191670
      const Triangle& t = ts[index];
266
267
28766680
      for (Triangle::index_type j = 0; j < 3; ++j) {
268
21575010
        Triangle::index_type point_id = t[j];
269
21575010
        const Vec3f& p = ps[point_id];
270


21575010
        Vec3f v(p[0], p[1], p[2]);
271

21575010
        P[P_id][0] = axes.col(0).dot(v);
272

21575010
        P[P_id][1] = axes.col(1).dot(v);
273

21575010
        P[P_id][2] = axes.col(2).dot(v);
274
21575010
        P_id++;
275
      }
276
277
7191670
      if (ps2) {
278
        for (Triangle::index_type j = 0; j < 3; ++j) {
279
          Triangle::index_type point_id = t[j];
280
          const Vec3f& p = ps2[point_id];
281
          // FIXME Is this right ?????
282
          Vec3f v(p[0], p[1], p[2]);
283
          P[P_id][0] = axes.col(0).dot(v);
284
          P[P_id][1] = axes.col(0).dot(v);
285
          P[P_id][2] = axes.col(1).dot(v);
286
          P_id++;
287
        }
288
      }
289
    }
290
  } else {
291
276759
    for (unsigned int i = 0; i < n; ++i) {
292
250378
      unsigned int index = indirect_index ? indices[i] : i;
293
294
250378
      const Vec3f& p = ps[index];
295


250378
      Vec3f v(p[0], p[1], p[2]);
296

250378
      P[P_id][0] = axes.col(0).dot(v);
297

250378
      P[P_id][1] = axes.col(1).dot(v);
298

250378
      P[P_id][2] = axes.col(2).dot(v);
299
250378
      P_id++;
300
301
250378
      if (ps2) {
302
        const Vec3f& v = ps2[index];
303
        P[P_id][0] = axes.col(0).dot(v);
304
        P[P_id][1] = axes.col(1).dot(v);
305
        P[P_id][2] = axes.col(2).dot(v);
306
        P_id++;
307
      }
308
    }
309
  }
310
311
  FCL_REAL minx, maxx, miny, maxy, minz, maxz;
312
313
  FCL_REAL cz, radsqr;
314
315
1335766
  minz = maxz = P[0][2];
316
317
21825388
  for (unsigned int i = 1; i < size_P; ++i) {
318
20489622
    FCL_REAL z_value = P[i][2];
319
20489622
    if (z_value < minz)
320
1668573
      minz = z_value;
321
18821049
    else if (z_value > maxz)
322
1650791
      maxz = z_value;
323
  }
324
325
1335766
  r = (FCL_REAL)0.5 * (maxz - minz);
326
1335766
  radsqr = r * r;
327
1335766
  cz = (FCL_REAL)0.5 * (maxz + minz);
328
329
  // compute an initial norm of rectangle along x direction
330
331
  // find minx and maxx as starting points
332
333
1335766
  unsigned int minindex = 0, maxindex = 0;
334
  FCL_REAL mintmp, maxtmp;
335
1335766
  mintmp = maxtmp = P[0][0];
336
337
21825388
  for (unsigned int i = 1; i < size_P; ++i) {
338
20489622
    FCL_REAL x_value = P[i][0];
339
20489622
    if (x_value < mintmp) {
340
1720586
      minindex = i;
341
1720586
      mintmp = x_value;
342
18769036
    } else if (x_value > maxtmp) {
343
1759224
      maxindex = i;
344
1759224
      maxtmp = x_value;
345
    }
346
  }
347
348
  FCL_REAL x, dz;
349
1335766
  dz = P[minindex][2] - cz;
350
1335766
  minx = P[minindex][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
351
1335766
  dz = P[maxindex][2] - cz;
352
1335766
  maxx = P[maxindex][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
353
354
  // grow minx/maxx
355
356
23161154
  for (unsigned int i = 0; i < size_P; ++i) {
357
21825388
    if (P[i][0] < minx) {
358
3130279
      dz = P[i][2] - cz;
359
3130279
      x = P[i][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
360
3130279
      if (x < minx) minx = x;
361
18695109
    } else if (P[i][0] > maxx) {
362
3097454
      dz = P[i][2] - cz;
363
3097454
      x = P[i][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
364
3097454
      if (x > maxx) maxx = x;
365
    }
366
  }
367
368
  // compute an initial norm of rectangle along y direction
369
370
  // find miny and maxy as starting points
371
372
1335766
  minindex = maxindex = 0;
373
1335766
  mintmp = maxtmp = P[0][1];
374
21825388
  for (unsigned int i = 1; i < size_P; ++i) {
375
20489622
    FCL_REAL y_value = P[i][1];
376
20489622
    if (y_value < mintmp) {
377
2366846
      minindex = i;
378
2366846
      mintmp = y_value;
379
18122776
    } else if (y_value > maxtmp) {
380
1711318
      maxindex = i;
381
1711318
      maxtmp = y_value;
382
    }
383
  }
384
385
  FCL_REAL y;
386
1335766
  dz = P[minindex][2] - cz;
387
1335766
  miny = P[minindex][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
388
1335766
  dz = P[maxindex][2] - cz;
389
1335766
  maxy = P[maxindex][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
390
391
  // grow miny/maxy
392
393
23161154
  for (unsigned int i = 0; i < size_P; ++i) {
394
21825388
    if (P[i][1] < miny) {
395
3037848
      dz = P[i][2] - cz;
396
3037848
      y = P[i][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
397
3037848
      if (y < miny) miny = y;
398
18787540
    } else if (P[i][1] > maxy) {
399
3018530
      dz = P[i][2] - cz;
400
3018530
      y = P[i][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
401
3018530
      if (y > maxy) maxy = y;
402
    }
403
  }
404
405
  // corners may have some points which are not covered - grow lengths if
406
  // necessary quite conservative (can be improved)
407
  FCL_REAL dx, dy, u, t;
408
1335766
  FCL_REAL a = std::sqrt((FCL_REAL)0.5);
409
23161154
  for (unsigned int i = 0; i < size_P; ++i) {
410
21825388
    if (P[i][0] > maxx) {
411
2810376
      if (P[i][1] > maxy) {
412
451253
        dx = P[i][0] - maxx;
413
451253
        dy = P[i][1] - maxy;
414
451253
        u = dx * a + dy * a;
415
451253
        t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) +
416
451253
            (cz - P[i][2]) * (cz - P[i][2]);
417
451253
        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
418
451253
        if (u > 0) {
419
94203
          maxx += u * a;
420
94203
          maxy += u * a;
421
        }
422
2359123
      } else if (P[i][1] < miny) {
423
453390
        dx = P[i][0] - maxx;
424
453390
        dy = P[i][1] - miny;
425
453390
        u = dx * a - dy * a;
426
453390
        t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) +
427
453390
            (cz - P[i][2]) * (cz - P[i][2]);
428
453390
        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
429
453390
        if (u > 0) {
430
99637
          maxx += u * a;
431
99637
          miny -= u * a;
432
        }
433
      }
434
19015012
    } else if (P[i][0] < minx) {
435
2814641
      if (P[i][1] > maxy) {
436
434752
        dx = P[i][0] - minx;
437
434752
        dy = P[i][1] - maxy;
438
434752
        u = dy * a - dx * a;
439
434752
        t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) +
440
434752
            (cz - P[i][2]) * (cz - P[i][2]);
441
434752
        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
442
434752
        if (u > 0) {
443
90920
          minx -= u * a;
444
90920
          maxy += u * a;
445
        }
446
2379889
      } else if (P[i][1] < miny) {
447
439485
        dx = P[i][0] - minx;
448
439485
        dy = P[i][1] - miny;
449
439485
        u = -dx * a - dy * a;
450
439485
        t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) +
451
439485
            (cz - P[i][2]) * (cz - P[i][2]);
452
439485
        u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
453
439485
        if (u > 0) {
454
109445
          minx -= u * a;
455
109445
          miny -= u * a;
456
        }
457
      }
458
    }
459
  }
460
461


1335766
  origin.noalias() = axes * Vec3f(minx, miny, cz);
462
463
1335766
  l[0] = std::max<FCL_REAL>(maxx - minx, 0);
464
1335766
  l[1] = std::max<FCL_REAL>(maxy - miny, 0);
465
466
1335766
  delete[] P;
467
1335766
}
468
469
/** @brief Compute the bounding volume extent and center for a set or subset of
470
 * points. The bounding volume axes are known.
471
 */
472
19868
static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
473
                                                 unsigned int* indices,
474
                                                 unsigned int n, Matrix3f& axes,
475
                                                 Vec3f& center, Vec3f& extent) {
476
19868
  bool indirect_index = true;
477
19868
  if (!indices) indirect_index = false;
478
479
19868
  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
480
481
19868
  Vec3f min_coord(real_max, real_max, real_max);
482
19868
  Vec3f max_coord(-real_max, -real_max, -real_max);
483
484
81018
  for (unsigned int i = 0; i < n; ++i) {
485
61150
    unsigned int index = indirect_index ? indices[i] : i;
486
487
61150
    const Vec3f& p = ps[index];
488

61150
    Vec3f proj(axes.transpose() * p);
489
490
244600
    for (int j = 0; j < 3; ++j) {
491


183450
      if (proj[j] > max_coord[j]) max_coord[j] = proj[j];
492


183450
      if (proj[j] < min_coord[j]) min_coord[j] = proj[j];
493
    }
494
495
61150
    if (ps2) {
496
      const Vec3f& v = ps2[index];
497
      proj.noalias() = axes.transpose() * v;
498
499
      for (int j = 0; j < 3; ++j) {
500
        if (proj[j] > max_coord[j]) max_coord[j] = proj[j];
501
        if (proj[j] < min_coord[j]) min_coord[j] = proj[j];
502
      }
503
    }
504
  }
505
506


19868
  center.noalias() = axes * (max_coord + min_coord) / 2;
507
508


19868
  extent.noalias() = (max_coord - min_coord) / 2;
509
19868
}
510
511
/** @brief Compute the bounding volume extent and center for a set or subset of
512
 * points. The bounding volume axes are known.
513
 */
514
1467062
static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
515
                                           unsigned int* indices,
516
                                           unsigned int n, Matrix3f& axes,
517
                                           Vec3f& center, Vec3f& extent) {
518
1467062
  bool indirect_index = true;
519
1467062
  if (!indices) indirect_index = false;
520
521
1467062
  FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
522
523
1467062
  Vec3f min_coord(real_max, real_max, real_max);
524
1467062
  Vec3f max_coord(-real_max, -real_max, -real_max);
525
526
9665898
  for (unsigned int i = 0; i < n; ++i) {
527
8198836
    unsigned int index = indirect_index ? indices[i] : i;
528
8198836
    const Triangle& t = ts[index];
529
530
32795344
    for (Triangle::index_type j = 0; j < 3; ++j) {
531
24596508
      Triangle::index_type point_id = t[j];
532
24596508
      const Vec3f& p = ps[point_id];
533

24596508
      Vec3f proj(axes.transpose() * p);
534
535
98386032
      for (int k = 0; k < 3; ++k) {
536


73789524
        if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
537


73789524
        if (proj[k] < min_coord[k]) min_coord[k] = proj[k];
538
      }
539
    }
540
541
8198836
    if (ps2) {
542
      for (Triangle::index_type j = 0; j < 3; ++j) {
543
        Triangle::index_type point_id = t[j];
544
        const Vec3f& p = ps2[point_id];
545
        Vec3f proj(axes.transpose() * p);
546
547
        for (int k = 0; k < 3; ++k) {
548
          if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
549
          if (proj[k] < min_coord[k]) min_coord[k] = proj[k];
550
        }
551
      }
552
    }
553
  }
554
555

1467062
  Vec3f o((max_coord + min_coord) / 2);
556
557

1467062
  center.noalias() = axes * o;
558
559


1467062
  extent.noalias() = (max_coord - min_coord) / 2;
560
1467062
}
561
562
1486930
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts,
563
                        unsigned int* indices, unsigned int n, Matrix3f& axes,
564
                        Vec3f& center, Vec3f& extent) {
565
1486930
  if (ts)
566
1467062
    getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent);
567
  else
568
19868
    getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent);
569
1486930
}
570
571
6540
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c,
572
                             Vec3f& center, FCL_REAL& radius) {
573

6540
  Vec3f e1 = a - c;
574

6540
  Vec3f e2 = b - c;
575
6540
  FCL_REAL e1_len2 = e1.squaredNorm();
576
6540
  FCL_REAL e2_len2 = e2.squaredNorm();
577
6540
  Vec3f e3 = e1.cross(e2);
578
6540
  FCL_REAL e3_len2 = e3.squaredNorm();
579

6540
  radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
580
6540
  radius = std::sqrt(radius) * 0.5;
581
582



6540
  center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
583
6540
}
584
585
667935
static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
586
                                            unsigned int* indices,
587
                                            unsigned int n,
588
                                            const Vec3f& query) {
589
667935
  bool indirect_index = true;
590
667935
  if (!indices) indirect_index = false;
591
592
667935
  FCL_REAL maxD = 0;
593
4008555
  for (unsigned int i = 0; i < n; ++i) {
594
3340620
    unsigned int index = indirect_index ? indices[i] : i;
595
3340620
    const Triangle& t = ts[index];
596
597
13362480
    for (Triangle::index_type j = 0; j < 3; ++j) {
598
10021860
      Triangle::index_type point_id = t[j];
599
10021860
      const Vec3f& p = ps[point_id];
600
601
10021860
      FCL_REAL d = (p - query).squaredNorm();
602
10021860
      if (d > maxD) maxD = d;
603
    }
604
605
3340620
    if (ps2) {
606
      for (Triangle::index_type j = 0; j < 3; ++j) {
607
        Triangle::index_type point_id = t[j];
608
        const Vec3f& p = ps2[point_id];
609
610
        FCL_REAL d = (p - query).squaredNorm();
611
        if (d > maxD) maxD = d;
612
      }
613
    }
614
  }
615
616
667935
  return std::sqrt(maxD);
617
}
618
619
3
static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2,
620
                                                  unsigned int* indices,
621
                                                  unsigned int n,
622
                                                  const Vec3f& query) {
623
3
  bool indirect_index = true;
624
3
  if (!indices) indirect_index = false;
625
626
3
  FCL_REAL maxD = 0;
627
39
  for (unsigned int i = 0; i < n; ++i) {
628
36
    unsigned int index = indirect_index ? indices[i] : i;
629
630
36
    const Vec3f& p = ps[index];
631
36
    FCL_REAL d = (p - query).squaredNorm();
632
36
    if (d > maxD) maxD = d;
633
634
36
    if (ps2) {
635
      const Vec3f& v = ps2[index];
636
      FCL_REAL d = (v - query).squaredNorm();
637
      if (d > maxD) maxD = d;
638
    }
639
  }
640
641
3
  return std::sqrt(maxD);
642
}
643
644
667938
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
645
                         unsigned int* indices, unsigned int n,
646
                         const Vec3f& query) {
647
667938
  if (ts)
648
667935
    return maximumDistance_mesh(ps, ps2, ts, indices, n, query);
649
  else
650
3
    return maximumDistance_pointcloud(ps, ps2, indices, n, query);
651
}
652
653
}  // namespace fcl
654
655
}  // namespace hpp