narrowphase.cpp 88.8 KB
Newer Older
jpan's avatar
jpan committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
jpan's avatar
jpan committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
jpan's avatar
jpan committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

38
39
40
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/intersect.h>
jpan's avatar
jpan committed
41
42
43
44
45
46
47
48
#include <boost/math/constants/constants.hpp>
#include <vector>

namespace fcl
{

namespace details
{
49
50
51
52
53
// Compute the point on a line segment that is the closest point on the
// segment to to another point. The code is inspired by the explanation
// given by Dan Sunday's page:
//   http://geomalgorithms.com/a02-_lines.html
static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) {
54
55
  Vec3f v = s2 - s1;
  Vec3f w = p - s1;
56

57
58
  FCL_REAL c1 = w.dot(v);
  FCL_REAL c2 = v.dot(v);
59

60
61
62
63
64
65
66
67
68
  if (c1 <= 0) {
    sp = s1;
  } else if (c2 <= c1) {
    sp = s2;
  } else {
    FCL_REAL b = c1/c2;
    Vec3f Pb = s1 + v * b;
    sp = Pb;
  }
69
70
}

71
72
bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1, 
                            const Capsule& s2, const Transform3f& tf2,
73
                            Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
74
{
75
76
  Transform3f tf2_inv (tf2);
  tf2_inv.inverse();
77
78
79
80
  
  Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function
  Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
  Vec3f s_c = tf1.getTranslation ();
81

82
  Vec3f segment_point;
83

84
85
  lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
  Vec3f diff = s_c - segment_point;
86

87
88
  FCL_REAL diffN = diff.norm();
  FCL_REAL distance = diffN - s1.radius - s2.radius;
89

90
91
  if (distance > 0)
    return false;
92

93
  // Vec3f normal (-diff.normalized());
94

95
96
  if (distance < 0 && penetration_depth)
    *penetration_depth = -distance;
97

98
  if (normal_)
99
    *normal_ = -diff / diffN;
100

101
  if (contact_points) {
102
    *contact_points = segment_point + diff * s2.radius;
103
  }
104

105
  return true;
Martin Felis's avatar
Martin Felis committed
106
107
108
}

bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, 
109
110
                           const Capsule& s2, const Transform3f& tf2,
                           FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
Martin Felis's avatar
Martin Felis committed
111
{
112
113
  Transform3f tf2_inv(tf2);
  tf2_inv.inverse();
Martin Felis's avatar
Martin Felis committed
114

115
116
117
  Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
  Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
  Vec3f s_c = tf1.getTranslation ();
Martin Felis's avatar
Martin Felis committed
118

119
  Vec3f segment_point;
Martin Felis's avatar
Martin Felis committed
120

121
122
  lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
  Vec3f diff = s_c - segment_point;
Martin Felis's avatar
Martin Felis committed
123

124
  FCL_REAL distance = diff.norm() - s1.radius - s2.radius;
Martin Felis's avatar
Martin Felis committed
125

126
  if(dist) *dist = distance;
127

128
129
130
131
132
133
134
135
  if(p1 || p2) diff.normalize();
  if(p1)
  {
    *p1 = s_c - diff * s1.radius;
  }

  if(p2) *p2 = segment_point + diff * s1.radius;

136
137
138
  if(distance <= 0)
    return false;

139
  return true;
140
141
}

142
143
bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, 
                           const Sphere& s2, const Transform3f& tf2,
144
                           Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
jpan's avatar
jpan committed
145
{
Joseph Mirabel's avatar
Joseph Mirabel committed
146
  Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
147
  FCL_REAL len = diff.norm();
jpan's avatar
jpan committed
148
149
150
151
152
  if(len > s1.radius + s2.radius)
    return false;

  if(penetration_depth) 
    *penetration_depth = s1.radius + s2.radius - len;
153
154
155

  // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
  // Otherwise, normal is pointing from center of object 1 to center of object 2
jpan's avatar
jpan committed
156
157
158
159
160
161
162
163
164
  if(normal) 
  {
    if(len > 0)
      *normal = diff / len;
    else
      *normal = diff;
  }

  if(contact_points)
Joseph Mirabel's avatar
Joseph Mirabel committed
165
    *contact_points = tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius);
jpan's avatar
jpan committed
166
167
168
169
170
  
  return true;
}


171
172
bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
                          const Sphere& s2, const Transform3f& tf2,
173
                          FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
jpan's avatar
jpan committed
174
{
175
176
177
  Vec3f o1 = tf1.getTranslation();
  Vec3f o2 = tf2.getTranslation();
  Vec3f diff = o1 - o2;
178
  FCL_REAL len = diff.norm();
179
  FCL_REAL d (len > s1.radius + s2.radius);
jpan's avatar
jpan committed
180

181
182
183
184
185
  if(dist) *dist = len - (s1.radius + s2.radius);
  if(p1) *p1 = o1 - diff * (s1.radius / len);
  if(p2) *p2 = o2 + diff * (s2.radius / len);

  return (d >=0);
jpan's avatar
jpan committed
186
187
}

188
/** \brief the minimum distance from a point to a line */
189
FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest) 
jpan's avatar
jpan committed
190
191
192
{
  Vec3f diff = p - from;
  Vec3f v = to - from;
193
  FCL_REAL t = v.dot(diff);
jpan's avatar
jpan committed
194
195
196
	
  if(t > 0) 
  {
197
    FCL_REAL dotVV = v.dot(v);
jpan's avatar
jpan committed
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
    if(t < dotVV) 
    {
      t /= dotVV;
      diff -= v * t;
    } 
    else 
    {
      t = 1;
      diff -= v;
    }
  } 
  else
    t = 0;

  nearest = from + v * t;
  return diff.dot(diff);	
}

216
/// @brief Whether a point's projection is in a triangle
217
bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& normal, const Vec3f& p)
jpan's avatar
jpan committed
218
219
220
221
222
223
224
225
226
227
228
229
230
{
  Vec3f edge1(p2 - p1);
  Vec3f edge2(p3 - p2);
  Vec3f edge3(p1 - p3);

  Vec3f p1_to_p(p - p1);
  Vec3f p2_to_p(p - p2);
  Vec3f p3_to_p(p - p3);

  Vec3f edge1_normal(edge1.cross(normal));
  Vec3f edge2_normal(edge2.cross(normal));
  Vec3f edge3_normal(edge3.cross(normal));
	
231
  FCL_REAL r1, r2, r3;
jpan's avatar
jpan committed
232
233
234
235
236
237
238
239
240
241
  r1 = edge1_normal.dot(p1_to_p);
  r2 = edge2_normal.dot(p2_to_p);
  r3 = edge3_normal.dot(p3_to_p);
  if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
       ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
    return true;
  return false;
}


242
bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
243
                             const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
jpan's avatar
jpan committed
244
245
{
  Vec3f normal = (P2 - P1).cross(P3 - P1);
246
  normal.normalize();
jpan's avatar
jpan committed
247
  const Vec3f& center = tf.getTranslation();
248
249
  const FCL_REAL& radius = s.radius;
  FCL_REAL radius_with_threshold = radius + std::numeric_limits<FCL_REAL>::epsilon();
jpan's avatar
jpan committed
250
  Vec3f p1_to_center = center - P1;
251
  FCL_REAL distance_from_plane = p1_to_center.dot(normal);
jpan's avatar
jpan committed
252
253
254
255
256
257
258
259
260
261
262
263
264

  if(distance_from_plane < 0)
  {
    distance_from_plane *= -1;
    normal *= -1;
  }

  bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold);
  
  bool has_contact = false;
  Vec3f contact_point;
  if(is_inside_contact_plane)
  {
265
    if(projectInTriangle(P1, P2, P3, normal, center))
jpan's avatar
jpan committed
266
267
268
269
270
271
    {
      has_contact = true;
      contact_point = center - normal * distance_from_plane;
    }
    else
    {
272
      FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
jpan's avatar
jpan committed
273
      Vec3f nearest_on_edge;
274
      FCL_REAL distance_sqr;
jpan's avatar
jpan committed
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
      distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
      if(distance_sqr < contact_capsule_radius_sqr)
      {
        has_contact = true;
        contact_point = nearest_on_edge;
      }

      distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
      if(distance_sqr < contact_capsule_radius_sqr)
      {
        has_contact = true;
        contact_point = nearest_on_edge;
      }

      distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
      if(distance_sqr < contact_capsule_radius_sqr)
      {
        has_contact = true;
        contact_point = nearest_on_edge;
      }
    }
  }

  if(has_contact)
  {
300
    Vec3f contact_to_center = contact_point - center;
301
    FCL_REAL distance_sqr = contact_to_center.squaredNorm();
jpan's avatar
jpan committed
302
303
304
305
306

    if(distance_sqr < radius_with_threshold * radius_with_threshold)
    {
      if(distance_sqr > 0)
      {
307
        FCL_REAL distance = std::sqrt(distance_sqr);
308
        if(normal_) *normal_ = contact_to_center.normalized();
jpan's avatar
jpan committed
309
310
311
312
313
        if(contact_points) *contact_points = contact_point;
        if(penetration_depth) *penetration_depth = -(radius - distance);
      }
      else
      {
314
        if(normal_) *normal_ = -normal;
jpan's avatar
jpan committed
315
316
317
318
319
320
321
322
323
324
325
        if(contact_points) *contact_points = contact_point;
        if(penetration_depth) *penetration_depth = -radius;
      }

      return true;
    }
  }

  return false;
}

326

327
bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
328
                            const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
329
                            FCL_REAL* dist)
330
331
332
333
{
  // from geometric tools, very different from the collision code.

  const Vec3f& center = tf.getTranslation();
334
  FCL_REAL radius = sp.radius;
335
336
337
  Vec3f diff = P1 - center;
  Vec3f edge0 = P2 - P1;
  Vec3f edge1 = P3 - P1;
338
  FCL_REAL a00 = edge0.squaredNorm();
339
  FCL_REAL a01 = edge0.dot(edge1);
340
  FCL_REAL a11 = edge1.squaredNorm();
341
342
  FCL_REAL b0 = diff.dot(edge0);
  FCL_REAL b1 = diff.dot(edge1);
343
  FCL_REAL c = diff.squaredNorm();
344
345
346
347
348
  FCL_REAL det = fabs(a00*a11 - a01*a01);
  FCL_REAL s = a01*b1 - a11*b0;
  FCL_REAL t = a01*b0 - a00*b1;

  FCL_REAL sqr_dist;
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431

  if(s + t <= det)
  {
    if(s < 0)
    {
      if(t < 0)  // region 4
      {
        if(b0 < 0)
        {
          t = 0;
          if(-b0 >= a00)
          {
            s = 1;
            sqr_dist = a00 + 2*b0 + c;
          }
          else
          {
            s = -b0/a00;
            sqr_dist = b0*s + c;
          }
        }
        else
        {
          s = 0;
          if(b1 >= 0)
          {
            t = 0;
            sqr_dist = c;
          }
          else if(-b1 >= a11)
          {
            t = 1;
            sqr_dist = a11 + 2*b1 + c;
          }
          else
          {
            t = -b1/a11;
            sqr_dist = b1*t + c;
          }
        }
      }
      else  // region 3
      {
        s = 0;
        if(b1 >= 0)
        {
          t = 0;
          sqr_dist = c;
        }
        else if(-b1 >= a11)
        {
          t = 1;
          sqr_dist = a11 + 2*b1 + c;
        }
        else
        {
          t = -b1/a11;
          sqr_dist = b1*t + c;
        }
      }
    }
    else if(t < 0)  // region 5
    {
      t = 0;
      if(b0 >= 0)
      {
        s = 0;
        sqr_dist = c;
      }
      else if(-b0 >= a00)
      {
        s = 1;
        sqr_dist = a00 + 2*b0 + c;
      }
      else
      {
        s = -b0/a00;
        sqr_dist = b0*s + c;
      }
    }
    else  // region 0
    {
      // minimum at interior point
432
      FCL_REAL inv_det = (1)/det;
433
434
435
436
437
438
439
      s *= inv_det;
      t *= inv_det;
      sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
    }
  }
  else
  {
440
    FCL_REAL tmp0, tmp1, numer, denom;
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557

    if(s < 0)  // region 2
    {
      tmp0 = a01 + b0;
      tmp1 = a11 + b1;
      if(tmp1 > tmp0)
      {
        numer = tmp1 - tmp0;
        denom = a00 - 2*a01 + a11;
        if(numer >= denom)
        {
          s = 1;
          t = 0;
          sqr_dist = a00 + 2*b0 + c;
        }
        else
        {
          s = numer/denom;
          t = 1 - s;
          sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
        }
      }
      else
      {
        s = 0;
        if(tmp1 <= 0)
        {
          t = 1;
          sqr_dist = a11 + 2*b1 + c;
        }
        else if(b1 >= 0)
        {
          t = 0;
          sqr_dist = c;
        }
        else
        {
          t = -b1/a11;
          sqr_dist = b1*t + c;
        }
      }
    }
    else if(t < 0)  // region 6
    {
      tmp0 = a01 + b1;
      tmp1 = a00 + b0;
      if(tmp1 > tmp0)
      {
        numer = tmp1 - tmp0;
        denom = a00 - 2*a01 + a11;
        if(numer >= denom)
        {
          t = 1;
          s = 0;
          sqr_dist = a11 + 2*b1 + c;
        }
        else
        {
          t = numer/denom;
          s = 1 - t;
          sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
        }
      }
      else
      {
        t = 0;
        if(tmp1 <= 0)
        {
          s = 1;
          sqr_dist = a00 + 2*b0 + c;
        }
        else if(b0 >= 0)
        {
          s = 0;
          sqr_dist = c;
        }
        else
        {
          s = -b0/a00;
          sqr_dist = b0*s + c;
        }
      }
    }
    else  // region 1
    {
      numer = a11 + b1 - a01 - b0;
      if(numer <= 0)
      {
        s = 0;
        t = 1;
        sqr_dist = a11 + 2*b1 + c;
      }
      else
      {
        denom = a00 - 2*a01 + a11;
        if(numer >= denom)
        {
          s = 1;
          t = 0;
          sqr_dist = a00 + 2*b0 + c;
        }
        else
        {
          s = numer/denom;
          t = 1 - s;
          sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
        }
      }
    }
  }

  // Account for numerical round-off error.
  if(sqr_dist < 0)
    sqr_dist = 0;

  if(sqr_dist > radius * radius)
  {
558
    if(dist) *dist = std::sqrt(sqr_dist) - radius;
559
560
561
562
    return true;
  }
  else
  {
563
    if(dist) *dist = -1;
564
565
566
567
    return false;
  }
}

jpan's avatar
jpan committed
568

569
570
571
572
573
574
575
576
577
578
579
bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
                            const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
                            FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
{
  if(p1 || p2)
  {
    Vec3f o = tf.getTranslation();
    Project::ProjectResult result;
    result = Project::projectTriangle(P1, P2, P3, o);
    if(result.sqr_distance > sp.radius * sp.radius)
    {
580
      if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
581
582
583
      Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
      Vec3f dir = o - project_p;
      dir.normalize();
584
      if(p1) { *p1 = o - dir * sp.radius; }
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
      if(p2) *p2 = project_p;
      return true;
    }
    else
      return false;
  }
  else
  {
    return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
  }
}


bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1,
                            const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
                            FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
{
602
  bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2);
603
604
605
  return res;
}

jpan's avatar
jpan committed
606

607
  
jpan's avatar
jpan committed
608
609
610
611
struct ContactPoint
{
  Vec3f normal;
  Vec3f point;
612
613
  FCL_REAL depth;
  ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
jpan's avatar
jpan committed
614
615
616
617
618
};


static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
                                       const Vec3f& pb, const Vec3f& ub,
619
                                       FCL_REAL* alpha, FCL_REAL* beta)
jpan's avatar
jpan committed
620
621
{
  Vec3f p = pb - pa;
622
623
624
625
626
  FCL_REAL uaub = ua.dot(ub);
  FCL_REAL q1 = ua.dot(p);
  FCL_REAL q2 = -ub.dot(p);
  FCL_REAL d = 1 - uaub * uaub;
  if(d <= (FCL_REAL)(0.0001f))
jpan's avatar
jpan committed
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
  {
    *alpha = 0;
    *beta = 0;
  }
  else
  {
    d = 1 / d;
    *alpha = (q1 + uaub * q2) * d;
    *beta = (uaub * q1 + q2) * d;
  }
}

// find all the intersection points between the 2D rectangle with vertices
// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
//
// the intersection points are returned as x,y pairs in the 'ret' array.
// the number of intersection points is returned by the function (this will
// be in the range 0 to 8).
646
static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
jpan's avatar
jpan committed
647
648
649
650
{
  // q (and r) contain nq (and nr) coordinate points for the current (and
  // chopped) polygons
  int nq = 4, nr = 0;
651
652
653
  FCL_REAL buffer[16];
  FCL_REAL* q = p;
  FCL_REAL* r = ret;
jpan's avatar
jpan committed
654
655
656
657
658
659
  for(int dir = 0; dir <= 1; ++dir) 
  {
    // direction notation: xy[0] = x axis, xy[1] = y axis
    for(int sign = -1; sign <= 1; sign += 2) 
    {
      // chop q along the line xy[dir] = sign*h[dir]
660
661
      FCL_REAL* pq = q;
      FCL_REAL* pr = r;
jpan's avatar
jpan committed
662
663
664
      nr = 0;
      for(int i = nq; i > 0; --i) 
      {
665
666
        // go through all points in q and all lines between adjacent points
        if(sign * pq[dir] < h[dir]) 
jpan's avatar
jpan committed
667
        {
668
669
670
671
672
673
          // this point is inside the chopping line
          pr[0] = pq[0];
          pr[1] = pq[1];
          pr += 2;
          nr++;
          if(nr & 8) 
jpan's avatar
jpan committed
674
          {
675
676
677
678
679
680
            q = r;
            goto done;
          }
        }
        FCL_REAL* nextq = (i > 1) ? pq+2 : q;
        if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) 
jpan's avatar
jpan committed
681
        {
682
683
684
685
686
687
688
          // this line crosses the chopping line
          pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
            (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
          pr[dir] = sign*h[dir];
          pr += 2;
          nr++;
          if(nr & 8) 
jpan's avatar
jpan committed
689
          {
690
691
692
693
694
            q = r;
            goto done;
          }
        }
        pq += 2;
jpan's avatar
jpan committed
695
696
697
698
699
700
701
702
      }
      q = r;
      r = (q == ret) ? buffer : ret;
      nq = nr;
    }
  }
 
 done:
703
  if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
jpan's avatar
jpan committed
704
705
706
707
708
709
710
711
712
713
  return nr;  
}

// given n points in the plane (array p, of size 2*n), generate m points that
// best represent the whole set. the definition of 'best' here is not
// predetermined - the idea is to select points that give good box-box
// collision detection behavior. the chosen point indexes are returned in the
// array iret (of size m). 'i0' is always the first entry in the array.
// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
// in the range [0..n-1].
714
static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
jpan's avatar
jpan committed
715
716
{
  // compute the centroid of the polygon in cx,cy
717
  FCL_REAL a, cx, cy, q;
jpan's avatar
jpan committed
718
719
720
721
722
723
724
725
726
727
728
729
730
731
  switch(n)
  {
  case 1:
    cx = p[0];
    cy = p[1];
    break;
  case 2:
    cx = 0.5 * (p[0] + p[2]);
    cy = 0.5 * (p[1] + p[3]);
    break;
  default:
    a = 0;
    cx = 0;
    cy = 0;
732
    for(int i = 0; i < n-1; ++i) 
jpan's avatar
jpan committed
733
734
735
736
737
738
739
    {
      q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
      a += q;
      cx += q*(p[i*2]+p[i*2+2]);
      cy += q*(p[i*2+1]+p[i*2+3]);
    }
    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
740
    if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
jpan's avatar
jpan committed
741
742
743
744
745
746
747
748
749
750
      a = 1/(3*(a+q));
    else
      a= 1e18f;
    
    cx = a*(cx + q*(p[n*2-2]+p[0]));
    cy = a*(cy + q*(p[n*2-1]+p[1]));
  }


  // compute the angle of each point w.r.t. the centroid
751
  FCL_REAL A[8];
752
  for(int i = 0; i < n; ++i) 
jpan's avatar
jpan committed
753
754
755
756
    A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);

  // search for points that have angles closest to A[i0] + i*(2*pi/m).
  int avail[8];
757
  for(int i = 0; i < n; ++i) avail[i] = 1;
jpan's avatar
jpan committed
758
759
760
  avail[i0] = 0;
  iret[0] = i0;
  iret++;
761
  const double pi = boost::math::constants::pi<FCL_REAL>();
762
  for(int j = 1; j < m; ++j) 
jpan's avatar
jpan committed
763
764
765
  {
    a = j*(2*pi/m) + A[i0];
    if (a > pi) a -= 2*pi;
766
    FCL_REAL maxdiff= 1e9, diff;
jpan's avatar
jpan committed
767
768

    *iret = i0;	// iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
769
    for(int i = 0; i < n; ++i) 
jpan's avatar
jpan committed
770
771
772
    {
      if(avail[i]) 
      {
773
774
775
        diff = std::abs(A[i]-a);
        if(diff > pi) diff = 2*pi - diff;
        if(diff < maxdiff) 
jpan's avatar
jpan committed
776
        {
777
778
779
          maxdiff = diff;
          *iret = i;
        }
jpan's avatar
jpan committed
780
781
782
783
784
785
786
787
788
789
790
      }
    }
    avail[*iret] = 0;
    iret++;
  }
}



int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
            const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
791
            Vec3f& normal, FCL_REAL* depth, int* return_code,
jpan's avatar
jpan committed
792
793
            int maxc, std::vector<ContactPoint>& contacts)
{
794
  const FCL_REAL fudge_factor = FCL_REAL(1.05);
795
  Vec3f normalC;
796
  FCL_REAL s, s2, l;
797
  int invert_normal, code;
jpan's avatar
jpan committed
798

799
800
  Vec3f p (T2 - T1); // get vector from centers of box 1 to box 2, relative to box 1
  Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1
jpan's avatar
jpan committed
801
802

  // get side lengths / 2
803
804
  Vec3f A (side1 * 0.5);
  Vec3f B (side2 * 0.5);
jpan's avatar
jpan committed
805
806

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
807
808
  Matrix3f R (R1.transpose() * R2);
  Matrix3f Q (R.cwiseAbs());
jpan's avatar
jpan committed
809
810
811
812
813
814
815
816
817
818
819
820
821


  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

  int best_col_id = -1;
822
  const Matrix3f* normalR = 0;
823
  FCL_REAL tmp = 0;
jpan's avatar
jpan committed
824

825
  s = - std::numeric_limits<FCL_REAL>::max();
jpan's avatar
jpan committed
826
827
828
829
830
  invert_normal = 0;
  code = 0;

  // separating axis = u1, u2, u3
  tmp = pp[0];
831
  s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
832
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
833
834
835
836
  if(s2 > s) 
  {
    s = s2; 
    best_col_id = 0;
837
    normalR = &R1;
jpan's avatar
jpan committed
838
839
840
841
842
    invert_normal = (tmp < 0);
    code = 1;
  }

  tmp = pp[1]; 
843
  s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
844
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
845
846
847
848
  if(s2 > s) 
  {
    s = s2; 
    best_col_id = 1;
849
    normalR = &R1;
jpan's avatar
jpan committed
850
851
852
853
854
    invert_normal = (tmp < 0);
    code = 2;
  }

  tmp = pp[2];
855
  s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
856
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
857
858
859
860
  if(s2 > s)
  {
    s = s2;
    best_col_id = 2;
861
    normalR = &R1;
jpan's avatar
jpan committed
862
863
864
865
866
    invert_normal = (tmp < 0);
    code = 3;
  }

  // separating axis = v1, v2, v3
867
868
  tmp = R2.col(0).dot(p);
  s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
869
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
870
871
872
873
  if(s2 > s)
  {
    s = s2;
    best_col_id = 0;
874
875
    normalR = &R2;
    invert_normal = (tmp < 0);
jpan's avatar
jpan committed
876
877
878
    code = 4;
  }

879
880
  tmp = R2.col(1).dot(p);
  s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
881
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
882
883
884
885
  if(s2 > s)
  {
    s = s2;
    best_col_id = 1;
886
887
    normalR = &R2;
    invert_normal = (tmp < 0);
jpan's avatar
jpan committed
888
889
890
    code = 5;
  }

891
892
  tmp = R2.col(2).dot(p);
  s2 =  std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
893
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
894
895
896
897
  if(s2 > s)
  {
    s = s2;
    best_col_id = 2;
898
899
    normalR = &R2;
    invert_normal = (tmp < 0);
jpan's avatar
jpan committed
900
901
902
903
    code = 6;
  }
  

904
  FCL_REAL fudge2(1.0e-6);
905
  Q.array() += fudge2;
jpan's avatar
jpan committed
906
907

  Vec3f n;
908
  FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
jpan's avatar
jpan committed
909
910

  // separating axis = u1 x (v1,v2,v3)
jpan's avatar
jpan committed
911
912
  tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
  s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
913
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
914
  n = Vec3f(0, -R(2, 0), R(1, 0));
915
  l = n.norm();
jpan's avatar
jpan committed
916
917
918
919
920
921
  if(l > eps) 
  {
    s2 /= l;
    if(s2 * fudge_factor > s)
    {
      s = s2;
922
      best_col_id = -1;
jpan's avatar
jpan committed
923
924
925
926
927
928
      normalC = n / l;
      invert_normal = (tmp < 0);
      code = 7;
    }
  }

jpan's avatar
jpan committed
929
930
  tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
  s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
931
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
932
  n = Vec3f(0, -R(2, 1), R(1, 1));
933
  l = n.norm();
jpan's avatar
jpan committed
934
935
936
937
938
939
  if(l > eps) 
  {
    s2 /= l;
    if(s2 * fudge_factor > s)
    {
      s = s2;
940
      best_col_id = -1;
jpan's avatar
jpan committed
941
942
943
944
945
946
      normalC = n / l;
      invert_normal = (tmp < 0);
      code = 8;
    }
  }
  
jpan's avatar
jpan committed
947
948
  tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
  s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
949
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
950
  n = Vec3f(0, -R(2, 2), R(1, 2));
951
  l = n.norm();
jpan's avatar
jpan committed
952
953
954
955
956
957
  if(l > eps) 
  {
    s2 /= l;
    if(s2 * fudge_factor > s)
    {
      s = s2;
958
      best_col_id = -1;
jpan's avatar
jpan committed
959
960
961
962
963
964
965
      normalC = n / l;
      invert_normal = (tmp < 0);
      code = 9;
    }
  }

  // separating axis = u2 x (v1,v2,v3)
jpan's avatar
jpan committed
966
967
  tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
  s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
968
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
969
  n = Vec3f(R(2, 0), 0, -R(0, 0));
970
  l = n.norm();
jpan's avatar
jpan committed
971
972
973
974
975
976
  if(l > eps) 
  {
    s2 /= l;
    if(s2 * fudge_factor > s)
    {
      s = s2;
977
      best_col_id = -1;
jpan's avatar
jpan committed
978
979
980
981
982
983
      normalC = n / l;
      invert_normal = (tmp < 0);
      code = 10;
    }
  }

jpan's avatar
jpan committed
984
985
  tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
  s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
986
  if(s2 > 0) { *return_code = 0; return 0; }
jpan's avatar
jpan committed
987
  n = Vec3f(R(2, 1), 0, -R(0, 1));
988
  l = n.norm();
jpan's avatar
jpan committed
989
990
991
992
993
994
  if(l > eps) 
  {
    s2 /= l;
    if(s2 * fudge_factor > s)
    {
      s = s2;
995
      best_col_id = -1;
jpan's avatar
jpan committed
996
997
998
999
1000
      normalC = n / l;
      invert_normal = (tmp < 0);
      code = 11;
    }
  }
For faster browsing, not all history is shown. View entire blame