GCC Code Coverage Report


Directory: ./
File: test/benchmark.cpp
Date: 2025-04-01 09:23:31
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