Directory: | ./ |
---|---|
File: | src/contact_patch.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 19 | 57 | 33.3% |
Branches: | 21 | 204 | 10.3% |
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.h" | ||
38 | #include "coal/collision_utility.h" | ||
39 | |||
40 | #include "coal/tracy.hh" | ||
41 | |||
42 | namespace coal { | ||
43 | |||
44 | 24 | ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() { | |
45 |
4/8✓ Branch 0 taken 2 times.
✓ Branch 1 taken 22 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
24 | static ContactPatchFunctionMatrix table; |
46 | 24 | return table; | |
47 | } | ||
48 | |||
49 | 25 | void computeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, | |
50 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
51 | const CollisionResult& collision_result, | ||
52 | const ContactPatchRequest& request, | ||
53 | ContactPatchResult& result) { | ||
54 | COAL_TRACY_ZONE_SCOPED_N("coal::computeContactPatch"); | ||
55 |
5/6✓ Branch 1 taken 24 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 24 times.
|
25 | if (!collision_result.isCollision() || request.max_num_patch == 0) { |
56 | // do nothing | ||
57 | 1 | return; | |
58 | } | ||
59 | |||
60 | // Before doing any computation, we initialize and clear the input result. | ||
61 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | result.set(request); |
62 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | ContactPatchSolver csolver(request); |
63 | |||
64 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | OBJECT_TYPE object_type1 = o1->getObjectType(); |
65 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | OBJECT_TYPE object_type2 = o2->getObjectType(); |
66 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | NODE_TYPE node_type1 = o1->getNodeType(); |
67 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | NODE_TYPE node_type2 = o2->getNodeType(); |
68 | |||
69 | const ContactPatchFunctionMatrix& looktable = | ||
70 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | getContactPatchFunctionLookTable(); |
71 | |||
72 |
2/4✓ Branch 0 taken 24 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
24 | if (object_type1 == OT_GEOM && |
73 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
|
24 | (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { |
74 | ✗ | if (!looktable.contact_patch_matrix[node_type2][node_type1]) { | |
75 | ✗ | COAL_THROW_PRETTY("Computing contact patches between node type " | |
76 | << std::string(get_node_type_name(node_type1)) | ||
77 | << " and node type " | ||
78 | << std::string(get_node_type_name(node_type2)) | ||
79 | << " is not yet supported.", | ||
80 | std::invalid_argument); | ||
81 | } | ||
82 | ✗ | looktable.contact_patch_matrix[node_type2][node_type1]( | |
83 | o2, tf2, o1, tf1, collision_result, &csolver, request, result); | ||
84 | ✗ | result.swapObjects(); | |
85 | ✗ | return; | |
86 | } | ||
87 | |||
88 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 24 times.
|
24 | if (!looktable.contact_patch_matrix[node_type1][node_type2]) { |
89 | ✗ | COAL_THROW_PRETTY("Contact patch computation between node type " | |
90 | << std::string(get_node_type_name(node_type1)) | ||
91 | << " and node type " | ||
92 | << std::string(get_node_type_name(node_type2)) | ||
93 | << " is not yet supported.", | ||
94 | std::invalid_argument); | ||
95 | } | ||
96 | |||
97 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | return looktable.contact_patch_matrix[node_type1][node_type2]( |
98 | 24 | o1, tf1, o2, tf2, collision_result, &csolver, request, result); | |
99 | 24 | } | |
100 | |||
101 | ✗ | void computeContactPatch(const CollisionObject* o1, const CollisionObject* o2, | |
102 | const CollisionResult& collision_result, | ||
103 | const ContactPatchRequest& request, | ||
104 | ContactPatchResult& result) { | ||
105 | ✗ | return computeContactPatch(o1->collisionGeometryPtr(), o1->getTransform(), | |
106 | o2->collisionGeometryPtr(), o2->getTransform(), | ||
107 | ✗ | collision_result, request, result); | |
108 | } | ||
109 | |||
110 | ✗ | ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, | |
111 | ✗ | const CollisionGeometry* o2) | |
112 | ✗ | : o1(o1), o2(o2) { | |
113 | const ContactPatchFunctionMatrix& looktable = | ||
114 | ✗ | getContactPatchFunctionLookTable(); | |
115 | |||
116 | ✗ | OBJECT_TYPE object_type1 = this->o1->getObjectType(); | |
117 | ✗ | NODE_TYPE node_type1 = this->o1->getNodeType(); | |
118 | ✗ | OBJECT_TYPE object_type2 = this->o2->getObjectType(); | |
119 | ✗ | NODE_TYPE node_type2 = this->o2->getNodeType(); | |
120 | |||
121 | ✗ | this->swap_geoms = object_type1 == OT_GEOM && | |
122 | ✗ | (object_type2 == OT_BVH || object_type2 == OT_HFIELD); | |
123 | ✗ | if ((this->swap_geoms && | |
124 | ✗ | !looktable.contact_patch_matrix[node_type2][node_type1]) || | |
125 | ✗ | (!this->swap_geoms && | |
126 | ✗ | !looktable.contact_patch_matrix[node_type1][node_type2])) { | |
127 | ✗ | COAL_THROW_PRETTY("Collision function between node type " | |
128 | << std::string(get_node_type_name(node_type1)) | ||
129 | << " and node type " | ||
130 | << std::string(get_node_type_name(node_type2)) | ||
131 | << " is not yet supported.", | ||
132 | std::invalid_argument); | ||
133 | } | ||
134 | |||
135 | ✗ | if (this->swap_geoms) { | |
136 | ✗ | this->func = looktable.contact_patch_matrix[node_type2][node_type1]; | |
137 | } else { | ||
138 | ✗ | this->func = looktable.contact_patch_matrix[node_type1][node_type2]; | |
139 | } | ||
140 | } | ||
141 | |||
142 | ✗ | void ComputeContactPatch::run(const Transform3s& tf1, const Transform3s& tf2, | |
143 | const CollisionResult& collision_result, | ||
144 | const ContactPatchRequest& request, | ||
145 | ContactPatchResult& result) const { | ||
146 | COAL_TRACY_ZONE_SCOPED_N("coal::ComputeContactPatch::run"); | ||
147 | ✗ | if (!collision_result.isCollision() || request.max_num_patch == 0) { | |
148 | // do nothing | ||
149 | ✗ | return; | |
150 | } | ||
151 | |||
152 | // Before doing any computation, we initialize and clear the input result. | ||
153 | ✗ | result.set(request); | |
154 | ✗ | if (this->swap_geoms) { | |
155 | ✗ | this->func(this->o2, tf2, this->o1, tf1, collision_result, &(this->csolver), | |
156 | request, result); | ||
157 | ✗ | result.swapObjects(); | |
158 | } else { | ||
159 | ✗ | this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver), | |
160 | request, result); | ||
161 | } | ||
162 | } | ||
163 | |||
164 | ✗ | void ComputeContactPatch::operator()(const Transform3s& tf1, | |
165 | const Transform3s& tf2, | ||
166 | const CollisionResult& collision_result, | ||
167 | const ContactPatchRequest& request, | ||
168 | ContactPatchResult& result) const | ||
169 | |||
170 | { | ||
171 | ✗ | this->csolver.set(request); | |
172 | ✗ | this->run(tf1, tf2, collision_result, request, result); | |
173 | } | ||
174 | |||
175 | } // namespace coal | ||
176 |