GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2018-2019, CNRS |
||
5 |
* Author: Florent Lamiraux |
||
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 |
#include <cmath> |
||
37 |
#include <limits> |
||
38 |
#include <hpp/fcl/math/transform.h> |
||
39 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
40 |
#include <hpp/fcl/internal/shape_shape_func.h> |
||
41 |
#include <hpp/fcl/internal/traversal_node_base.h> |
||
42 |
|||
43 |
// Note that partial specialization of template functions is not allowed. |
||
44 |
// Therefore, two implementations with the default narrow phase solvers are |
||
45 |
// provided. If another narrow phase solver were to be used, the default |
||
46 |
// template implementation would be called, unless the function is also |
||
47 |
// specialized for this new type. |
||
48 |
// |
||
49 |
// One solution would be to make narrow phase solvers derive from an abstract |
||
50 |
// class and specialize the template for this abstract class. |
||
51 |
namespace hpp { |
||
52 |
namespace fcl { |
||
53 |
struct GJKSolver; |
||
54 |
|||
55 |
template <> |
||
56 |
1038 |
FCL_REAL ShapeShapeDistance<Sphere, Sphere>( |
|
57 |
const CollisionGeometry* o1, const Transform3f& tf1, |
||
58 |
const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, |
||
59 |
const DistanceRequest&, DistanceResult& result) { |
||
60 |
1038 |
FCL_REAL epsilon = 1e-7; |
|
61 |
1038 |
const Sphere* s1 = static_cast<const Sphere*>(o1); |
|
62 |
1038 |
const Sphere* s2 = static_cast<const Sphere*>(o2); |
|
63 |
|||
64 |
// We assume that spheres are centered at the origin of their frame. |
||
65 |
1038 |
const fcl::Vec3f& center1 = tf1.getTranslation(); |
|
66 |
1038 |
const fcl::Vec3f& center2 = tf2.getTranslation(); |
|
67 |
1038 |
FCL_REAL r1 = s1->radius; |
|
68 |
1038 |
FCL_REAL r2 = s2->radius; |
|
69 |
|||
70 |
1038 |
result.o1 = o1; |
|
71 |
1038 |
result.o2 = o2; |
|
72 |
1038 |
result.b1 = result.b2 = -1; |
|
73 |
✓✗✓✗ |
1038 |
Vec3f c1c2 = center2 - center1; |
74 |
✓✗ | 1038 |
FCL_REAL dist = c1c2.norm(); |
75 |
✓✗ | 1038 |
Vec3f unit(0, 0, 0); |
76 |
✓✓✓✗ ✓✗ |
1038 |
if (dist > epsilon) unit = c1c2 / dist; |
77 |
FCL_REAL penetrationDepth; |
||
78 |
1038 |
penetrationDepth = r1 + r2 - dist; |
|
79 |
1038 |
bool collision = (penetrationDepth >= 0); |
|
80 |
1038 |
result.min_distance = -penetrationDepth; |
|
81 |
✓✓ | 1038 |
if (collision) { |
82 |
// Take contact point at the middle of intersection between each sphere |
||
83 |
// and segment [c1 c2]. |
||
84 |
227 |
FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); |
|
85 |
✓✗✓✗ ✓✗ |
227 |
Vec3f contact = center1 + abscissa * unit; |
86 |
✓✗✓✗ |
227 |
result.nearest_points[0] = result.nearest_points[1] = contact; |
87 |
227 |
return result.min_distance; |
|
88 |
} else { |
||
89 |
811 |
FCL_REAL abs1(r1), abs2(dist - r2); |
|
90 |
✓✗✓✗ ✓✗ |
811 |
result.nearest_points[0] = center1 + abs1 * unit; |
91 |
✓✗✓✗ ✓✗ |
811 |
result.nearest_points[1] = center1 + abs2 * unit; |
92 |
} |
||
93 |
811 |
return result.min_distance; |
|
94 |
} |
||
95 |
|||
96 |
71327 |
std::size_t ShapeShapeCollider<Sphere, Sphere>::run( |
|
97 |
const CollisionGeometry* o1, const Transform3f& tf1, |
||
98 |
const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, |
||
99 |
const CollisionRequest& request, CollisionResult& result) { |
||
100 |
71327 |
FCL_REAL epsilon = 1e-7; |
|
101 |
71327 |
const Sphere* s1 = static_cast<const Sphere*>(o1); |
|
102 |
71327 |
const Sphere* s2 = static_cast<const Sphere*>(o2); |
|
103 |
|||
104 |
// We assume that capsules are centered at the origin. |
||
105 |
71327 |
const fcl::Vec3f& center1 = tf1.getTranslation(); |
|
106 |
71327 |
const fcl::Vec3f& center2 = tf2.getTranslation(); |
|
107 |
71327 |
FCL_REAL r1 = s1->radius; |
|
108 |
71327 |
FCL_REAL r2 = s2->radius; |
|
109 |
71327 |
FCL_REAL margin = request.security_margin; |
|
110 |
|||
111 |
✓✗✓✗ |
71327 |
Vec3f c1c2 = center2 - center1; |
112 |
✓✗ | 71327 |
FCL_REAL dist = c1c2.norm(); |
113 |
✓✗ | 71327 |
Vec3f unit(0, 0, 0); |
114 |
✓✓✓✗ ✓✗ |
71327 |
if (dist > epsilon) unit = c1c2 / dist; |
115 |
// Unlike in distance computation, we consider the security margin. |
||
116 |
71327 |
FCL_REAL distToCollision = dist - (r1 + r2 + margin); |
|
117 |
|||
118 |
✓✗✓✗ ✓✗ |
142654 |
internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, |
119 |
✓✗✓✗ |
71327 |
center1 + unit * r1, |
120 |
✓✗✓✗ |
71327 |
center2 - unit * r2); |
121 |
✓✓ | 71327 |
if (distToCollision <= request.collision_distance_threshold) { |
122 |
// Take contact point at the middle of intersection between each sphere |
||
123 |
// and segment [c1 c2]. |
||
124 |
560 |
FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); |
|
125 |
✓✗✓✗ ✓✗ |
560 |
Vec3f contactPoint = center1 + abscissa * unit; |
126 |
Contact contact(o1, o2, -1, -1, contactPoint, unit, |
||
127 |
✓✗ | 560 |
-(distToCollision + margin)); |
128 |
✓✗ | 560 |
result.addContact(contact); |
129 |
560 |
return 1; |
|
130 |
} |
||
131 |
70767 |
return 0; |
|
132 |
} |
||
133 |
} // namespace fcl |
||
134 |
|||
135 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |