GCC Code Coverage Report

Directory: ./
File: test/benchmark.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 0 100 0.0%
Branches: 0 192 0.0%
Line Branch Exec Source
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of Coal.
5 // Coal is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // Coal is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // Coal. If not, see <http://www.gnu.org/licenses/>.
16
17 #include <boost/filesystem.hpp>
18
19 #include "coal/internal/traversal_node_setup.h"
20 #include "coal/internal/traversal_node_bvhs.h"
21 #include "../src/collision_node.h"
22 #include "coal/internal/BV_splitter.h"
23
24 #include "utility.h"
25 #include "fcl_resources/config.h"
26
27 #define RUN_CASE(BV, tf, models, split) \
28 run<BV>(tf, models, split, #BV " - " #split ":\t")
29
30 using namespace coal;
31
32 bool verbose = false;
33 Scalar DELTA = Scalar(0.001);
34
35 template <typename BV>
36 void makeModel(const std::vector<Vec3s>& vertices,
37 const std::vector<Triangle>& triangles,
38 SplitMethodType split_method, BVHModel<BV>& model);
39
40 template <typename BV, typename TraversalNode>
41 double distance(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
42 const BVHModel<BV>& m2, bool verbose);
43
44 template <typename BV, typename TraversalNode>
45 double collide(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
46 const BVHModel<BV>& m2, bool verbose);
47
48 template <typename BV>
49 double run(const std::vector<Transform3s>& tf,
50 const BVHModel<BV> (&models)[2][3], int split_method,
51 const char* sm_name);
52
53 template <typename BV>
54 struct traits {};
55
56 template <>
57 struct traits<RSS> {
58 typedef MeshCollisionTraversalNodeRSS CollisionTraversalNode;
59 typedef MeshDistanceTraversalNodeRSS DistanceTraversalNode;
60 };
61
62 template <>
63 struct traits<kIOS> {
64 typedef MeshCollisionTraversalNodekIOS CollisionTraversalNode;
65 typedef MeshDistanceTraversalNodekIOS DistanceTraversalNode;
66 };
67
68 template <>
69 struct traits<OBB> {
70 typedef MeshCollisionTraversalNodeOBB CollisionTraversalNode;
71 // typedef MeshDistanceTraversalNodeOBB DistanceTraversalNode;
72 };
73
74 template <>
75 struct traits<OBBRSS> {
76 typedef MeshCollisionTraversalNodeOBBRSS CollisionTraversalNode;
77 typedef MeshDistanceTraversalNodeOBBRSS DistanceTraversalNode;
78 };
79
80 template <typename BV>
81 ✗ void makeModel(const std::vector<Vec3s>& vertices,
82 const std::vector<Triangle>& triangles,
83 SplitMethodType split_method, BVHModel<BV>& model) {
84 ✗ model.bv_splitter.reset(new BVSplitter<BV>(split_method));
85 ✗ model.bv_splitter.reset(new BVSplitter<BV>(split_method));
86
87 ✗ model.beginModel();
88 ✗ model.addSubModel(vertices, triangles);
89 ✗ model.endModel();
90 }
91
92 template <typename BV, typename TraversalNode>
93 ✗ double distance(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
94 const BVHModel<BV>& m2, bool verbose) {
95 ✗ Transform3s pose2;
96
97 ✗ DistanceResult local_result;
98 ✗ DistanceRequest request(true);
99 ✗ TraversalNode node;
100
101 ✗ node.enable_statistics = verbose;
102
103 ✗ BenchTimer timer;
104 ✗ timer.start();
105
106 ✗ for (std::size_t i = 0; i < tf.size(); ++i) {
107 ✗ if (!initialize(node, m1, tf[i], m2, pose2, request, local_result))
108 ✗ std::cout << "initialize error" << std::endl;
109
110 ✗ distance(&node, NULL);
111 }
112 ✗ timer.stop();
113 ✗ return timer.getElapsedTimeInMicroSec();
114 }
115
116 template <typename BV, typename TraversalNode>
117 ✗ double collide(const std::vector<Transform3s>& tf, const BVHModel<BV>& m1,
118 const BVHModel<BV>& m2, bool verbose) {
119 ✗ Transform3s pose2;
120
121 ✗ CollisionResult local_result;
122 ✗ CollisionRequest request;
123 ✗ TraversalNode node(request);
124
125 ✗ node.enable_statistics = verbose;
126
127 ✗ BenchTimer timer;
128 ✗ timer.start();
129
130 ✗ for (std::size_t i = 0; i < tf.size(); ++i) {
131 ✗ bool success(initialize(node, m1, tf[i], m2, pose2, local_result));
132 (void)success;
133 ✗ assert(success);
134
135 ✗ CollisionResult result;
136 ✗ collide(&node, request, result);
137 }
138
139 ✗ timer.stop();
140 ✗ return timer.getElapsedTimeInMicroSec();
141 }
142
143 template <typename BV>
144 ✗ double run(const std::vector<Transform3s>& tf,
145 const BVHModel<BV> (&models)[2][3], int split_method,
146 const char* prefix) {
147 ✗ double col = collide<BV, typename traits<BV>::CollisionTraversalNode>(
148 ✗ tf, models[0][split_method], models[1][split_method], verbose);
149 ✗ double dist = distance<BV, typename traits<BV>::DistanceTraversalNode>(
150 ✗ tf, models[0][split_method], models[1][split_method], verbose);
151
152 ✗ std::cout << prefix << " (" << col << ", " << dist << ")\n";
153 ✗ return col + dist;
154 }
155
156 template <>
157 ✗ double run<OBB>(const std::vector<Transform3s>& tf,
158 const BVHModel<OBB> (&models)[2][3], int split_method,
159 const char* prefix) {
160 ✗ double col = collide<OBB, traits<OBB>::CollisionTraversalNode>(
161 ✗ tf, models[0][split_method], models[1][split_method], verbose);
162 ✗ double dist = 0;
163
164 ✗ std::cout << prefix << " (\t" << col << ", \tNaN)\n";
165 ✗ return col + dist;
166 }
167
168 ✗ int main(int, char*[]) {
169 ✗ std::vector<Vec3s> p1, p2;
170 ✗ std::vector<Triangle> t1, t2;
171 ✗ boost::filesystem::path path(TEST_RESOURCES_DIR);
172 ✗ loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
173 ✗ loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
174
175 // Make models
176 ✗ BVHModel<RSS> ms_rss[2][3];
177 ✗ makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_rss[0][SPLIT_METHOD_MEAN]);
178 ✗ makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_rss[0][SPLIT_METHOD_BV_CENTER]);
179 ✗ makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_rss[0][SPLIT_METHOD_MEDIAN]);
180 ✗ makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_rss[1][SPLIT_METHOD_MEAN]);
181 ✗ makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_rss[1][SPLIT_METHOD_BV_CENTER]);
182 ✗ makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_rss[1][SPLIT_METHOD_MEDIAN]);
183
184 ✗ BVHModel<kIOS> ms_kios[2][3];
185 ✗ makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_kios[0][SPLIT_METHOD_MEAN]);
186 ✗ makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_kios[0][SPLIT_METHOD_BV_CENTER]);
187 ✗ makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_kios[0][SPLIT_METHOD_MEDIAN]);
188 ✗ makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_kios[1][SPLIT_METHOD_MEAN]);
189 ✗ makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_kios[1][SPLIT_METHOD_BV_CENTER]);
190 ✗ makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_kios[1][SPLIT_METHOD_MEDIAN]);
191
192 ✗ BVHModel<OBB> ms_obb[2][3];
193 ✗ makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obb[0][SPLIT_METHOD_MEAN]);
194 ✗ makeModel(p1, t1, SPLIT_METHOD_BV_CENTER, ms_obb[0][SPLIT_METHOD_BV_CENTER]);
195 ✗ makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obb[0][SPLIT_METHOD_MEDIAN]);
196 ✗ makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obb[1][SPLIT_METHOD_MEAN]);
197 ✗ makeModel(p2, t2, SPLIT_METHOD_BV_CENTER, ms_obb[1][SPLIT_METHOD_BV_CENTER]);
198 ✗ makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obb[1][SPLIT_METHOD_MEDIAN]);
199
200 ✗ BVHModel<OBBRSS> ms_obbrss[2][3];
201 ✗ makeModel(p1, t1, SPLIT_METHOD_MEAN, ms_obbrss[0][SPLIT_METHOD_MEAN]);
202 ✗ makeModel(p1, t1, SPLIT_METHOD_BV_CENTER,
203 ms_obbrss[0][SPLIT_METHOD_BV_CENTER]);
204 ✗ makeModel(p1, t1, SPLIT_METHOD_MEDIAN, ms_obbrss[0][SPLIT_METHOD_MEDIAN]);
205 ✗ makeModel(p2, t2, SPLIT_METHOD_MEAN, ms_obbrss[1][SPLIT_METHOD_MEAN]);
206 ✗ makeModel(p2, t2, SPLIT_METHOD_BV_CENTER,
207 ms_obbrss[1][SPLIT_METHOD_BV_CENTER]);
208 ✗ makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]);
209
210 ✗ std::vector<Transform3s> transforms; // t0
211 ✗ Scalar extents[] = {-3000, -3000, -3000, 3000, 3000, 3000};
212 ✗ std::size_t n = 10000;
213
214 ✗ generateRandomTransforms(extents, transforms, n);
215 ✗ double total_time = 0;
216
217 ✗ total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEAN);
218 ✗ total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_BV_CENTER);
219 ✗ total_time += RUN_CASE(RSS, transforms, ms_rss, SPLIT_METHOD_MEDIAN);
220
221 ✗ total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEAN);
222 ✗ total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_BV_CENTER);
223 ✗ total_time += RUN_CASE(kIOS, transforms, ms_kios, SPLIT_METHOD_MEDIAN);
224
225 ✗ total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEAN);
226 ✗ total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_BV_CENTER);
227 ✗ total_time += RUN_CASE(OBB, transforms, ms_obb, SPLIT_METHOD_MEDIAN);
228
229 ✗ total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEAN);
230 ✗ total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_BV_CENTER);
231 ✗ total_time += RUN_CASE(OBBRSS, transforms, ms_obbrss, SPLIT_METHOD_MEDIAN);
232
233 ✗ std::cout << "\n\nTotal time: " << total_time << std::endl;
234 }
235