Directory: | ./ |
---|---|
File: | src/contact_patch_func_matrix.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 243 | 271 | 89.7% |
Branches: | 4 | 58 | 6.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Software License Agreement (BSD License) | ||
3 | * | ||
4 | * Copyright (c) 2024, INRIA | ||
5 | * All rights reserved. | ||
6 | * | ||
7 | * Redistribution and use in source and binary forms, with or without | ||
8 | * modification, are permitted provided that the following conditions | ||
9 | * are met: | ||
10 | * | ||
11 | * * Redistributions of source code must retain the above copyright | ||
12 | * notice, this list of conditions and the following disclaimer. | ||
13 | * * Redistributions in binary form must reproduce the above | ||
14 | * copyright notice, this list of conditions and the following | ||
15 | * disclaimer in the documentation and/or other materials provided | ||
16 | * with the distribution. | ||
17 | * * Neither the name of INRIA nor the names of its | ||
18 | * contributors may be used to endorse or promote products derived | ||
19 | * from this software without specific prior written permission. | ||
20 | * | ||
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
32 | * POSSIBILITY OF SUCH DAMAGE. | ||
33 | */ | ||
34 | |||
35 | /** \author Louis Montaut */ | ||
36 | |||
37 | #include "coal/contact_patch_func_matrix.h" | ||
38 | #include "coal/shape/geometric_shapes.h" | ||
39 | #include "coal/internal/shape_shape_contact_patch_func.h" | ||
40 | #include "coal/BV/BV.h" | ||
41 | |||
42 | namespace coal { | ||
43 | |||
44 | template <typename T_BVH, typename T_SH> | ||
45 | struct BVHShapeComputeContactPatch { | ||
46 | ✗ | static void run(const CollisionGeometry* o1, const Transform3s& tf1, | |
47 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
48 | const CollisionResult& collision_result, | ||
49 | const ContactPatchSolver* csolver, | ||
50 | const ContactPatchRequest& request, | ||
51 | ContactPatchResult& result) { | ||
52 | COAL_UNUSED_VARIABLE(o1); | ||
53 | COAL_UNUSED_VARIABLE(tf1); | ||
54 | COAL_UNUSED_VARIABLE(o2); | ||
55 | COAL_UNUSED_VARIABLE(tf2); | ||
56 | COAL_UNUSED_VARIABLE(csolver); | ||
57 | ✗ | for (size_t i = 0; i < collision_result.numContacts(); ++i) { | |
58 | ✗ | if (i >= request.max_num_patch) { | |
59 | ✗ | break; | |
60 | } | ||
61 | ✗ | const Contact& contact = collision_result.getContact(i); | |
62 | ✗ | ContactPatch& contact_patch = result.getUnusedContactPatch(); | |
63 | ✗ | constructContactPatchFrameFromContact(contact, contact_patch); | |
64 | ✗ | contact_patch.addPoint(contact.pos); | |
65 | } | ||
66 | } | ||
67 | }; | ||
68 | |||
69 | template <typename BV, typename Shape> | ||
70 | struct HeightFieldShapeComputeContactPatch { | ||
71 | ✗ | static void run(const CollisionGeometry* o1, const Transform3s& tf1, | |
72 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
73 | const CollisionResult& collision_result, | ||
74 | const ContactPatchSolver* csolver, | ||
75 | const ContactPatchRequest& request, | ||
76 | ContactPatchResult& result) { | ||
77 | COAL_UNUSED_VARIABLE(o1); | ||
78 | COAL_UNUSED_VARIABLE(tf1); | ||
79 | COAL_UNUSED_VARIABLE(o2); | ||
80 | COAL_UNUSED_VARIABLE(tf2); | ||
81 | COAL_UNUSED_VARIABLE(csolver); | ||
82 | ✗ | for (size_t i = 0; i < collision_result.numContacts(); ++i) { | |
83 | ✗ | if (i >= request.max_num_patch) { | |
84 | ✗ | break; | |
85 | } | ||
86 | ✗ | const Contact& contact = collision_result.getContact(i); | |
87 | ✗ | ContactPatch& contact_patch = result.getUnusedContactPatch(); | |
88 | ✗ | constructContactPatchFrameFromContact(contact, contact_patch); | |
89 | ✗ | contact_patch.addPoint(contact.pos); | |
90 | } | ||
91 | } | ||
92 | }; | ||
93 | |||
94 | template <typename BV> | ||
95 | struct BVHComputeContactPatch { | ||
96 | ✗ | static void run(const CollisionGeometry* o1, const Transform3s& tf1, | |
97 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
98 | const CollisionResult& collision_result, | ||
99 | const ContactPatchSolver* csolver, | ||
100 | const ContactPatchRequest& request, | ||
101 | ContactPatchResult& result) { | ||
102 | COAL_UNUSED_VARIABLE(o1); | ||
103 | COAL_UNUSED_VARIABLE(tf1); | ||
104 | COAL_UNUSED_VARIABLE(o2); | ||
105 | COAL_UNUSED_VARIABLE(tf2); | ||
106 | COAL_UNUSED_VARIABLE(csolver); | ||
107 | ✗ | for (size_t i = 0; i < collision_result.numContacts(); ++i) { | |
108 | ✗ | if (i >= request.max_num_patch) { | |
109 | ✗ | break; | |
110 | } | ||
111 | ✗ | const Contact& contact = collision_result.getContact(i); | |
112 | ✗ | ContactPatch& contact_patch = result.getUnusedContactPatch(); | |
113 | ✗ | constructContactPatchFrameFromContact(contact, contact_patch); | |
114 | ✗ | contact_patch.addPoint(contact.pos); | |
115 | } | ||
116 | } | ||
117 | }; | ||
118 | |||
119 | ✗ | COAL_LOCAL void contact_patch_function_not_implemented( | |
120 | const CollisionGeometry* o1, const Transform3s& /*tf1*/, | ||
121 | const CollisionGeometry* o2, const Transform3s& /*tf2*/, | ||
122 | const CollisionResult& /*collision_result*/, | ||
123 | const ContactPatchSolver* /*csolver*/, | ||
124 | const ContactPatchRequest& /*request*/, ContactPatchResult& /*result*/) { | ||
125 | ✗ | NODE_TYPE node_type1 = o1->getNodeType(); | |
126 | ✗ | NODE_TYPE node_type2 = o2->getNodeType(); | |
127 | |||
128 | ✗ | COAL_THROW_PRETTY("Contact patch function between node type " | |
129 | << std::string(get_node_type_name(node_type1)) | ||
130 | << " and node type " | ||
131 | << std::string(get_node_type_name(node_type2)) | ||
132 | << " is not yet supported.", | ||
133 | std::invalid_argument); | ||
134 | } | ||
135 | |||
136 | 2 | ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { | |
137 |
2/2✓ Branch 0 taken 44 times.
✓ Branch 1 taken 2 times.
|
46 | for (int i = 0; i < NODE_COUNT; ++i) { |
138 |
2/2✓ Branch 0 taken 968 times.
✓ Branch 1 taken 44 times.
|
1012 | for (int j = 0; j < NODE_COUNT; ++j) contact_patch_matrix[i][j] = nullptr; |
139 | } | ||
140 | |||
141 | // clang-format off | ||
142 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeContactPatch<Box, Box>; | |
143 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeContactPatch<Box, Sphere>; | |
144 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeContactPatch<Box, Capsule>; | |
145 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeContactPatch<Box, Cone>; | |
146 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeContactPatch<Box, Cylinder>; | |
147 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeContactPatch<Box, ConvexBase>; | |
148 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeContactPatch<Box, Plane>; | |
149 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Box, Halfspace>; | |
150 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Box, Ellipsoid>; | |
151 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Box, TriangleP>; | |
152 | |||
153 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeContactPatch<Sphere, Box>; | |
154 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeContactPatch<Sphere, Sphere>; | |
155 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeContactPatch<Sphere, Capsule>; | |
156 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeContactPatch<Sphere, Cone>; | |
157 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeContactPatch<Sphere, Cylinder>; | |
158 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeContactPatch<Sphere, ConvexBase>; | |
159 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeContactPatch<Sphere, Plane>; | |
160 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Sphere, Halfspace>; | |
161 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Sphere, Ellipsoid>; | |
162 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Sphere, TriangleP>; | |
163 | |||
164 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeContactPatch<Ellipsoid, Box>; | |
165 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeContactPatch<Ellipsoid, Sphere>; | |
166 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeContactPatch<Ellipsoid, Capsule>; | |
167 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeContactPatch<Ellipsoid, Cone>; | |
168 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeContactPatch<Ellipsoid, Cylinder>; | |
169 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeContactPatch<Ellipsoid, ConvexBase>; | |
170 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeContactPatch<Ellipsoid, Plane>; | |
171 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Ellipsoid, Halfspace>; | |
172 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Ellipsoid, Ellipsoid>; | |
173 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Ellipsoid, TriangleP>; | |
174 | |||
175 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeContactPatch<Capsule, Box>; | |
176 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeContactPatch<Capsule, Sphere>; | |
177 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeContactPatch<Capsule, Capsule>; | |
178 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeContactPatch<Capsule, Cone>; | |
179 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeContactPatch<Capsule, Cylinder>; | |
180 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeContactPatch<Capsule, ConvexBase>; | |
181 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeContactPatch<Capsule, Plane>; | |
182 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Capsule, Halfspace>; | |
183 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Capsule, Ellipsoid>; | |
184 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Capsule, TriangleP>; | |
185 | |||
186 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeContactPatch<Cone, Box>; | |
187 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeContactPatch<Cone, Sphere>; | |
188 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeContactPatch<Cone, Capsule>; | |
189 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeContactPatch<Cone, Cone>; | |
190 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeContactPatch<Cone, Cylinder>; | |
191 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeContactPatch<Cone, ConvexBase>; | |
192 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeContactPatch<Cone, Plane>; | |
193 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Cone, Halfspace>; | |
194 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Cone, Ellipsoid>; | |
195 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Cone, TriangleP>; | |
196 | |||
197 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeContactPatch<Cylinder, Box>; | |
198 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeContactPatch<Cylinder, Sphere>; | |
199 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeContactPatch<Cylinder, Capsule>; | |
200 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeContactPatch<Cylinder, Cone>; | |
201 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeContactPatch<Cylinder, Cylinder>; | |
202 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeContactPatch<Cylinder, ConvexBase>; | |
203 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeContactPatch<Cylinder, Plane>; | |
204 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Cylinder, Halfspace>; | |
205 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Cylinder, Ellipsoid>; | |
206 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Cylinder, TriangleP>; | |
207 | |||
208 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeContactPatch<ConvexBase, Box>; | |
209 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeContactPatch<ConvexBase, Sphere>; | |
210 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeContactPatch<ConvexBase, Capsule>; | |
211 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeContactPatch<ConvexBase, Cone>; | |
212 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeContactPatch<ConvexBase, Cylinder>; | |
213 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeContactPatch<ConvexBase, ConvexBase>; | |
214 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeContactPatch<ConvexBase, Plane>; | |
215 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeContactPatch<ConvexBase, Halfspace>; | |
216 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<ConvexBase, Ellipsoid>; | |
217 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_TRIANGLE] = &ShapeShapeContactPatch<ConvexBase, TriangleP>; | |
218 | |||
219 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeContactPatch<Plane, Box>; | |
220 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeContactPatch<Plane, Sphere>; | |
221 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeContactPatch<Plane, Capsule>; | |
222 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeContactPatch<Plane, Cone>; | |
223 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeContactPatch<Plane, Cylinder>; | |
224 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeContactPatch<Plane, ConvexBase>; | |
225 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeContactPatch<Plane, Plane>; | |
226 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Plane, Halfspace>; | |
227 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Plane, Ellipsoid>; | |
228 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Plane, TriangleP>; | |
229 | |||
230 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeContactPatch<Halfspace, Box>; | |
231 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeContactPatch<Halfspace, Sphere>; | |
232 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeContactPatch<Halfspace, Capsule>; | |
233 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeContactPatch<Halfspace, Cone>; | |
234 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeContactPatch<Halfspace, Cylinder>; | |
235 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeContactPatch<Halfspace, ConvexBase>; | |
236 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeContactPatch<Halfspace, Plane>; | |
237 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeContactPatch<Halfspace, Halfspace>; | |
238 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<Halfspace, Ellipsoid>; | |
239 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_TRIANGLE] = &ShapeShapeContactPatch<Halfspace, TriangleP>; | |
240 | |||
241 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_BOX] = &ShapeShapeContactPatch<TriangleP, Box>; | |
242 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_SPHERE] = &ShapeShapeContactPatch<TriangleP, Sphere>; | |
243 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_CAPSULE] = &ShapeShapeContactPatch<TriangleP, Capsule>; | |
244 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONE] = &ShapeShapeContactPatch<TriangleP, Cone>; | |
245 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_CYLINDER] = &ShapeShapeContactPatch<TriangleP, Cylinder>; | |
246 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONVEX] = &ShapeShapeContactPatch<TriangleP, ConvexBase>; | |
247 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_PLANE] = &ShapeShapeContactPatch<TriangleP, Plane>; | |
248 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_HALFSPACE] = &ShapeShapeContactPatch<TriangleP, Halfspace>; | |
249 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch<TriangleP, Ellipsoid>; | |
250 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_TRIANGLE] = &ShapeShapeContactPatch<TriangleP, TriangleP>; | |
251 | |||
252 | // TODO(louis): properly handle non-convex shapes like BVH, Octrees and Hfields. | ||
253 | // The following functions work. However apart from the contact frame, these functions don't | ||
254 | // compute more information than a call to `collide`. | ||
255 | 2 | contact_patch_matrix[BV_AABB][GEOM_BOX] = &BVHShapeComputeContactPatch<AABB, Box>::run; | |
256 | 2 | contact_patch_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeComputeContactPatch<AABB, Sphere>::run; | |
257 | 2 | contact_patch_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<AABB, Capsule>::run; | |
258 | 2 | contact_patch_matrix[BV_AABB][GEOM_CONE] = &BVHShapeComputeContactPatch<AABB, Cone>::run; | |
259 | 2 | contact_patch_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<AABB, Cylinder>::run; | |
260 | 2 | contact_patch_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeComputeContactPatch<AABB, ConvexBase>::run; | |
261 | 2 | contact_patch_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeComputeContactPatch<AABB, Plane>::run; | |
262 | 2 | contact_patch_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<AABB, Halfspace>::run; | |
263 | 2 | contact_patch_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<AABB, Ellipsoid>::run; | |
264 | |||
265 | 2 | contact_patch_matrix[BV_OBB][GEOM_BOX] = &BVHShapeComputeContactPatch<OBB, Box>::run; | |
266 | 2 | contact_patch_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeComputeContactPatch<OBB, Sphere>::run; | |
267 | 2 | contact_patch_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<OBB, Capsule>::run; | |
268 | 2 | contact_patch_matrix[BV_OBB][GEOM_CONE] = &BVHShapeComputeContactPatch<OBB, Cone>::run; | |
269 | 2 | contact_patch_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<OBB, Cylinder>::run; | |
270 | 2 | contact_patch_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeComputeContactPatch<OBB, ConvexBase>::run; | |
271 | 2 | contact_patch_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeComputeContactPatch<OBB, Plane>::run; | |
272 | 2 | contact_patch_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<OBB, Halfspace>::run; | |
273 | 2 | contact_patch_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<OBB, Ellipsoid>::run; | |
274 | |||
275 | 2 | contact_patch_matrix[BV_RSS][GEOM_BOX] = &BVHShapeComputeContactPatch<RSS, Box>::run; | |
276 | 2 | contact_patch_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeComputeContactPatch<RSS, Sphere>::run; | |
277 | 2 | contact_patch_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<RSS, Capsule>::run; | |
278 | 2 | contact_patch_matrix[BV_RSS][GEOM_CONE] = &BVHShapeComputeContactPatch<RSS, Cone>::run; | |
279 | 2 | contact_patch_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<RSS, Cylinder>::run; | |
280 | 2 | contact_patch_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeComputeContactPatch<RSS, ConvexBase>::run; | |
281 | 2 | contact_patch_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeComputeContactPatch<RSS, Plane>::run; | |
282 | 2 | contact_patch_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<RSS, Halfspace>::run; | |
283 | 2 | contact_patch_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<RSS, Ellipsoid>::run; | |
284 | |||
285 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeComputeContactPatch<KDOP<16>, Box>::run; | |
286 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeComputeContactPatch<KDOP<16>, Sphere>::run; | |
287 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<KDOP<16>, Capsule>::run; | |
288 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeComputeContactPatch<KDOP<16>, Cone>::run; | |
289 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<KDOP<16>, Cylinder>::run; | |
290 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeComputeContactPatch<KDOP<16>, ConvexBase>::run; | |
291 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeComputeContactPatch<KDOP<16>, Plane>::run; | |
292 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<KDOP<16>, Halfspace>::run; | |
293 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<KDOP<16>, Ellipsoid>::run; | |
294 | |||
295 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeComputeContactPatch<KDOP<18>, Box>::run; | |
296 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeComputeContactPatch<KDOP<18>, Sphere>::run; | |
297 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<KDOP<18>, Capsule>::run; | |
298 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeComputeContactPatch<KDOP<18>, Cone>::run; | |
299 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<KDOP<18>, Cylinder>::run; | |
300 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeComputeContactPatch<KDOP<18>, ConvexBase>::run; | |
301 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeComputeContactPatch<KDOP<18>, Plane>::run; | |
302 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<KDOP<18>, Halfspace>::run; | |
303 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<KDOP<18>, Ellipsoid>::run; | |
304 | |||
305 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeComputeContactPatch<KDOP<24>, Box>::run; | |
306 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeComputeContactPatch<KDOP<24>, Sphere>::run; | |
307 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<KDOP<24>, Capsule>::run; | |
308 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeComputeContactPatch<KDOP<24>, Cone>::run; | |
309 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<KDOP<24>, Cylinder>::run; | |
310 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeComputeContactPatch<KDOP<24>, ConvexBase>::run; | |
311 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeComputeContactPatch<KDOP<24>, Plane>::run; | |
312 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<KDOP<24>, Halfspace>::run; | |
313 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<KDOP<24>, Ellipsoid>::run; | |
314 | |||
315 | 2 | contact_patch_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeComputeContactPatch<kIOS, Box>::run; | |
316 | 2 | contact_patch_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeComputeContactPatch<kIOS, Sphere>::run; | |
317 | 2 | contact_patch_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<kIOS, Capsule>::run; | |
318 | 2 | contact_patch_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeComputeContactPatch<kIOS, Cone>::run; | |
319 | 2 | contact_patch_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<kIOS, Cylinder>::run; | |
320 | 2 | contact_patch_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeComputeContactPatch<kIOS, ConvexBase>::run; | |
321 | 2 | contact_patch_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeComputeContactPatch<kIOS, Plane>::run; | |
322 | 2 | contact_patch_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<kIOS, Halfspace>::run; | |
323 | 2 | contact_patch_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<kIOS, Ellipsoid>::run; | |
324 | |||
325 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeComputeContactPatch<OBBRSS, Box>::run; | |
326 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeComputeContactPatch<OBBRSS, Sphere>::run; | |
327 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeComputeContactPatch<OBBRSS, Capsule>::run; | |
328 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeComputeContactPatch<OBBRSS, Cone>::run; | |
329 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeComputeContactPatch<OBBRSS, Cylinder>::run; | |
330 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeComputeContactPatch<OBBRSS, ConvexBase>::run; | |
331 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeComputeContactPatch<OBBRSS, Plane>::run; | |
332 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch<OBBRSS, Halfspace>::run; | |
333 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch<OBBRSS, Ellipsoid>::run; | |
334 | |||
335 | 2 | contact_patch_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeComputeContactPatch<AABB, Box>::run; | |
336 | 2 | contact_patch_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeComputeContactPatch<AABB, Sphere>::run; | |
337 | 2 | contact_patch_matrix[HF_AABB][GEOM_CAPSULE] = &HeightFieldShapeComputeContactPatch<AABB, Capsule>::run; | |
338 | 2 | contact_patch_matrix[HF_AABB][GEOM_CONE] = &HeightFieldShapeComputeContactPatch<AABB, Cone>::run; | |
339 | 2 | contact_patch_matrix[HF_AABB][GEOM_CYLINDER] = &HeightFieldShapeComputeContactPatch<AABB, Cylinder>::run; | |
340 | 2 | contact_patch_matrix[HF_AABB][GEOM_CONVEX] = &HeightFieldShapeComputeContactPatch<AABB, ConvexBase>::run; | |
341 | 2 | contact_patch_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeComputeContactPatch<AABB, Plane>::run; | |
342 | 2 | contact_patch_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeComputeContactPatch<AABB, Halfspace>::run; | |
343 | 2 | contact_patch_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeComputeContactPatch<AABB, Ellipsoid>::run; | |
344 | |||
345 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeComputeContactPatch<OBBRSS, Box>::run; | |
346 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeComputeContactPatch<OBBRSS, Sphere>::run; | |
347 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_CAPSULE] = &HeightFieldShapeComputeContactPatch<OBBRSS, Capsule>::run; | |
348 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_CONE] = &HeightFieldShapeComputeContactPatch<OBBRSS, Cone>::run; | |
349 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_CYLINDER] = &HeightFieldShapeComputeContactPatch<OBBRSS, Cylinder>::run; | |
350 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_CONVEX] = &HeightFieldShapeComputeContactPatch<OBBRSS, ConvexBase>::run; | |
351 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeComputeContactPatch<OBBRSS, Plane>::run; | |
352 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeComputeContactPatch<OBBRSS, Halfspace>::run; | |
353 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeComputeContactPatch<OBBRSS, Ellipsoid>::run; | |
354 | |||
355 | 2 | contact_patch_matrix[BV_AABB][BV_AABB] = &BVHComputeContactPatch<AABB>::run; | |
356 | 2 | contact_patch_matrix[BV_OBB][BV_OBB] = &BVHComputeContactPatch<OBB>::run; | |
357 | 2 | contact_patch_matrix[BV_RSS][BV_RSS] = &BVHComputeContactPatch<RSS>::run; | |
358 | 2 | contact_patch_matrix[BV_KDOP16][BV_KDOP16] = &BVHComputeContactPatch<KDOP<16>>::run; | |
359 | 2 | contact_patch_matrix[BV_KDOP18][BV_KDOP18] = &BVHComputeContactPatch<KDOP<18>>::run; | |
360 | 2 | contact_patch_matrix[BV_KDOP24][BV_KDOP24] = &BVHComputeContactPatch<KDOP<24>>::run; | |
361 | 2 | contact_patch_matrix[BV_kIOS][BV_kIOS] = &BVHComputeContactPatch<kIOS>::run; | |
362 | 2 | contact_patch_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHComputeContactPatch<OBBRSS>::run; | |
363 | |||
364 | // TODO(louis): octrees | ||
365 | #ifdef COAL_HAS_OCTOMAP | ||
366 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
367 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_BOX] = &contact_patch_function_not_implemented; | |
368 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_SPHERE] = &contact_patch_function_not_implemented; | |
369 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &contact_patch_function_not_implemented; | |
370 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_CONE] = &contact_patch_function_not_implemented; | |
371 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &contact_patch_function_not_implemented; | |
372 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_CONVEX] = &contact_patch_function_not_implemented; | |
373 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_PLANE] = &contact_patch_function_not_implemented; | |
374 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &contact_patch_function_not_implemented; | |
375 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &contact_patch_function_not_implemented; | |
376 | 2 | contact_patch_matrix[GEOM_OCTREE][GEOM_TRIANGLE] = &contact_patch_function_not_implemented; | |
377 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_AABB] = &contact_patch_function_not_implemented; | |
378 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_OBB] = &contact_patch_function_not_implemented; | |
379 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_RSS] = &contact_patch_function_not_implemented; | |
380 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_OBBRSS] = &contact_patch_function_not_implemented; | |
381 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_kIOS] = &contact_patch_function_not_implemented; | |
382 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_KDOP16] = &contact_patch_function_not_implemented; | |
383 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_KDOP18] = &contact_patch_function_not_implemented; | |
384 | 2 | contact_patch_matrix[GEOM_OCTREE][BV_KDOP24] = &contact_patch_function_not_implemented; | |
385 | 2 | contact_patch_matrix[GEOM_OCTREE][HF_AABB] = &contact_patch_function_not_implemented; | |
386 | 2 | contact_patch_matrix[GEOM_OCTREE][HF_OBBRSS] = &contact_patch_function_not_implemented; | |
387 | |||
388 | 2 | contact_patch_matrix[GEOM_BOX][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
389 | 2 | contact_patch_matrix[GEOM_SPHERE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
390 | 2 | contact_patch_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
391 | 2 | contact_patch_matrix[GEOM_CONE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
392 | 2 | contact_patch_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
393 | 2 | contact_patch_matrix[GEOM_CONVEX][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
394 | 2 | contact_patch_matrix[GEOM_PLANE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
395 | 2 | contact_patch_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
396 | 2 | contact_patch_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
397 | 2 | contact_patch_matrix[GEOM_TRIANGLE][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
398 | 2 | contact_patch_matrix[BV_AABB][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
399 | 2 | contact_patch_matrix[BV_OBB][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
400 | 2 | contact_patch_matrix[BV_RSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
401 | 2 | contact_patch_matrix[BV_OBBRSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
402 | 2 | contact_patch_matrix[BV_kIOS][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
403 | 2 | contact_patch_matrix[BV_KDOP16][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
404 | 2 | contact_patch_matrix[BV_KDOP18][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
405 | 2 | contact_patch_matrix[BV_KDOP24][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
406 | 2 | contact_patch_matrix[HF_AABB][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
407 | 2 | contact_patch_matrix[HF_OBBRSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; | |
408 | #endif | ||
409 | // clang-format on | ||
410 | 2 | } | |
411 | |||
412 | } // namespace coal | ||
413 |