GCC Code Coverage Report


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