GCC Code Coverage Report

Directory: ./
File: test/profiling.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 74 163 45.4%
Branches: 88 446 19.7%
Line Branch Exec Source
1 // Copyright (c) 2017, 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/fwd.hh"
20 #include "coal/collision.h"
21 #include "coal/BVH/BVH_model.h"
22 #include "coal/collision_utility.h"
23 #include "coal/shape/geometric_shapes.h"
24 #include "coal/collision_func_matrix.h"
25 #include "coal/narrowphase/narrowphase.h"
26 #include "coal/mesh_loader/assimp.h"
27 #include "utility.h"
28 #include "fcl_resources/config.h"
29
30 using namespace coal;
31
32 CollisionFunctionMatrix lookupTable;
33 45 bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) {
34 45 OBJECT_TYPE object_type1 = o1->getObjectType();
35 45 OBJECT_TYPE object_type2 = o2->getObjectType();
36 45 NODE_TYPE node_type1 = o1->getNodeType();
37 45 NODE_TYPE node_type2 = o2->getNodeType();
38
39
4/4
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 20 times.
✓ Branch 3 taken 15 times.
 45 if (object_type1 == OT_GEOM && object_type2 == OT_BVH)
40 20 return (lookupTable.collision_matrix[node_type2][node_type1] != NULL);
41 else
42 25 return (lookupTable.collision_matrix[node_type1][node_type2] != NULL);
43 }
44
45 template <typename BV /*, SplitMethodType split_method*/>
46 8 CollisionGeometryPtr_t objToGeom(const std::string& filename) {
47 8 std::vector<Vec3s> pt;
48 8 std::vector<Triangle> tri;
49
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 loadOBJFile(filename.c_str(), pt, tri);
50
51
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 BVHModel<BV>* model(new BVHModel<BV>());
52 // model->bv_splitter.reset(new BVSplitter<BV>(split_method));
53
54
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 model->beginModel();
55
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 model->addSubModel(pt, tri);
56
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 model->endModel();
57
58
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 16 return CollisionGeometryPtr_t(model);
59 8 }
60
61 template <typename BV /*, SplitMethodType split_method*/>
62 ✗ CollisionGeometryPtr_t meshToGeom(const std::string& filename,
63 const Vec3s& scale = Vec3s(1, 1, 1)) {
64 ✗ shared_ptr<BVHModel<BV> > model(new BVHModel<BV>());
65 ✗ loadPolyhedronFromResource(filename, scale, model);
66 ✗ return model;
67 }
68
69 struct Geometry {
70 std::string type;
71 CollisionGeometryPtr_t o;
72
73
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 Geometry(const std::string& t, CollisionGeometry* ob) : type(t), o(ob) {}
74 4 Geometry(const std::string& t, const CollisionGeometryPtr_t& ob)
75 4 : type(t), o(ob) {}
76 };
77
78 struct Results {
79 std::vector<CollisionResult> rs;
80 Eigen::VectorXd times; // micro-seconds
81
82
2/4
✓ Branch 2 taken 41 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 41 times.
✗ Branch 7 not taken.
 41 Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {}
83 };
84
85 41 void collide(const std::vector<Transform3s>& tf, const CollisionGeometry* o1,
86 const CollisionGeometry* o2, const CollisionRequest& request,
87 Results& results) {
88
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 Transform3s Id;
89
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 BenchTimer timer;
90
2/2
✓ Branch 1 taken 41 times.
✓ Branch 2 taken 41 times.
 82 for (std::size_t i = 0; i < tf.size(); ++i) {
91
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 timer.start();
92 /* int num_contact = */
93
1/2
✓ Branch 3 taken 41 times.
✗ Branch 4 not taken.
 41 collide(o1, tf[i], o2, Id, request, results.rs[i]);
94
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 timer.stop();
95
2/4
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 41 times.
✗ Branch 5 not taken.
 41 results.times[(Eigen::DenseIndex)i] = timer.getElapsedTimeInMicroSec();
96 }
97 41 }
98
99 const char* sep = ", ";
100
101 1 void printResultHeaders() {
102 std::cout << "Type 1" << sep << "Type 2" << sep << "mean time" << sep
103 1 << "time std dev" << sep << "min time" << sep << "max time"
104 1 << std::endl;
105 1 }
106
107 41 void printResults(const Geometry& g1, const Geometry& g2, const Results& rs) {
108 41 double mean = rs.times.mean();
109
1/2
✓ Branch 2 taken 41 times.
✗ Branch 3 not taken.
 41 double var = rs.times.cwiseAbs2().mean() - mean * mean;
110 41 std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var)
111 41 << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff()
112 41 << std::endl;
113 41 }
114
115 #ifndef NDEBUG // if debug mode
116 size_t Ntransform = 1;
117 #else
118 size_t Ntransform = 100;
119 #endif
120 Scalar limit = 20;
121 bool verbose = false;
122
123 #define OUT(x) \
124 if (verbose) std::cout << x << std::endl
125 #define CHECK_PARAM_NB(NB, NAME) \
126 if (iarg + NB >= argc) \
127 throw std::invalid_argument(#NAME " requires " #NB " numbers")
128 ✗ void handleParam(int& iarg, const int& argc, char** argv,
129 CollisionRequest& request) {
130 ✗ while (iarg < argc) {
131 ✗ std::string a(argv[iarg]);
132 ✗ if (a == "-nb_transform") {
133 ✗ CHECK_PARAM_NB(1, nb_transform);
134 ✗ Ntransform = (size_t)atoi(argv[iarg + 1]);
135 ✗ OUT("nb_transform = " << Ntransform);
136 ✗ iarg += 2;
137 ✗ } else if (a == "-enable_distance_lower_bound") {
138 ✗ CHECK_PARAM_NB(1, enable_distance_lower_bound);
139 ✗ request.enable_distance_lower_bound = bool(atoi(argv[iarg + 1]));
140 ✗ iarg += 2;
141 ✗ } else if (a == "-limit") {
142 ✗ CHECK_PARAM_NB(1, limit);
143 ✗ limit = Scalar(atof(argv[iarg + 1]));
144 ✗ iarg += 2;
145 ✗ } else if (a == "-verbose") {
146 ✗ verbose = true;
147 ✗ iarg += 1;
148 } else {
149 ✗ break;
150 }
151 }
152 }
153 #define CREATE_SHAPE_2(var, Name) \
154 CHECK_PARAM_NB(2, Name); \
155 var.reset( \
156 new Name(Scalar(atof(argv[iarg + 1])), Scalar(atof(argv[iarg + 2])))); \
157 iarg += 3;
158 ✗ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) {
159 ✗ if (iarg >= argc) throw std::invalid_argument("An argument is required.");
160 ✗ std::string a(argv[iarg]);
161 ✗ std::string type;
162 ✗ CollisionGeometryPtr_t o;
163 ✗ if (a == "-box") {
164 ✗ CHECK_PARAM_NB(3, Box);
165 ✗ o.reset(new Box(Scalar(atof(argv[iarg + 1])), Scalar(atof(argv[iarg + 2])),
166 ✗ Scalar(atof(argv[iarg + 3]))));
167 ✗ iarg += 4;
168 ✗ type = "box";
169 ✗ } else if (a == "-sphere") {
170 ✗ CHECK_PARAM_NB(1, Sphere);
171 ✗ o.reset(new Sphere(Scalar(atof(argv[iarg + 1]))));
172 ✗ iarg += 2;
173 ✗ type = "sphere";
174 ✗ } else if (a == "-mesh") {
175 ✗ CHECK_PARAM_NB(2, Mesh);
176 ✗ OUT("Loading " << argv[iarg + 2] << " as BVHModel<" << argv[iarg + 1]
177 << ">...");
178 ✗ if (strcmp(argv[iarg + 1], "obb") == 0) {
179 ✗ o = meshToGeom<OBB>(argv[iarg + 2]);
180 ✗ OUT("Mesh has " << dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris
181 << " triangles");
182 ✗ type = "mesh_obb";
183 ✗ } else if (strcmp(argv[iarg + 1], "obbrss") == 0) {
184 ✗ o = meshToGeom<OBBRSS>(argv[iarg + 2]);
185 ✗ OUT("Mesh has " << dynamic_pointer_cast<BVHModel<OBBRSS> >(o)->num_tris
186 << " triangles");
187 ✗ type = "mesh_obbrss";
188 } else
189 ✗ throw std::invalid_argument("BV type must be obb or obbrss");
190 ✗ OUT("done.");
191 ✗ iarg += 3;
192 ✗ if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
193 ✗ CHECK_PARAM_NB(6, Crop);
194 coal::AABB aabb(
195 ✗ Vec3s(Scalar(atof(argv[iarg + 1])), Scalar(atof(argv[iarg + 2])),
196 ✗ Scalar(atof(argv[iarg + 3]))),
197 ✗ Vec3s(Scalar(atof(argv[iarg + 4])), Scalar(atof(argv[iarg + 5])),
198 ✗ Scalar(atof(argv[iarg + 6]))));
199 ✗ OUT("Cropping " << aabb.min_.transpose() << " ---- "
200 << aabb.max_.transpose() << " ...");
201 ✗ o->computeLocalAABB();
202 ✗ OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- "
203 << o->aabb_local.max_.transpose() << " ...");
204 ✗ o.reset(extract(o.get(), Transform3s(), aabb));
205 ✗ if (!o) throw std::invalid_argument("Failed to crop.");
206 ✗ OUT("Crop has " << dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris
207 << " triangles");
208 ✗ iarg += 7;
209 }
210 ✗ } else if (a == "-capsule") {
211 ✗ CREATE_SHAPE_2(o, Capsule);
212 ✗ type = "capsule";
213 ✗ } else if (a == "-cone") {
214 ✗ CREATE_SHAPE_2(o, Cone);
215 ✗ type = "cone";
216 ✗ } else if (a == "-cylinder") {
217 ✗ CREATE_SHAPE_2(o, Cylinder);
218 ✗ type = "cylinder";
219 } else {
220 ✗ throw std::invalid_argument(std::string("Unknown argument: ") + a);
221 }
222 ✗ return Geometry(type, o);
223 }
224
225 1 int main(int argc, char** argv) {
226 1 std::vector<Transform3s> transforms;
227
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CollisionRequest request;
229
230
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (argc > 1) {
231 ✗ int iarg = 1;
232 ✗ handleParam(iarg, argc, argv, request);
233 ✗ Geometry first = makeGeomFromParam(iarg, argc, argv);
234 ✗ Geometry second = makeGeomFromParam(iarg, argc, argv);
235
236 ✗ Scalar extents[] = {-limit, -limit, -limit, limit, limit, limit};
237 ✗ generateRandomTransforms(extents, transforms, Ntransform);
238 ✗ printResultHeaders();
239 ✗ Results results(Ntransform);
240 ✗ collide(transforms, first.o.get(), second.o.get(), request, results);
241 ✗ printResults(first, second, results);
242 ✗ } else {
243 1 Scalar extents[] = {-limit, -limit, -limit, limit, limit, limit};
244
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 generateRandomTransforms(extents, transforms, Ntransform);
245
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 boost::filesystem::path path(TEST_RESOURCES_DIR);
246
247 1 std::vector<Geometry> geoms;
248
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 geoms.push_back(Geometry("Box", new Box(1, 2, 3)));
249
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 geoms.push_back(Geometry("Sphere", new Sphere(2)));
250
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 geoms.push_back(Geometry("Capsule", new Capsule(2, 1)));
251
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 geoms.push_back(Geometry("Cone", new Cone(2, 1)));
252
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1)));
253 // geoms.push_back(Geometry ("Plane" , new Plane
254 // (Vec3s(1,1,1).normalized(), 0) ));
255 // geoms.push_back(Geometry ("Halfspace" , new Halfspace
256 // (Vec3s(1,1,1).normalized(), 0) ));
257 // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP
258 // (Vec3s(0,1,0), Vec3s(0,0,1), Vec3s(-1,0,0)) ));
259
260
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 geoms.push_back(Geometry("rob BVHModel<OBB>",
261
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 objToGeom<OBB>((path / "rob.obj").string())));
262 // geoms.push_back(Geometry ("rob BVHModel<RSS>" , objToGeom<RSS> ((path
263 // / "rob.obj").string()))); geoms.push_back(Geometry ("rob BVHModel<kIOS>"
264 // , objToGeom<kIOS> ((path / "rob.obj").string())));
265
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 geoms.push_back(Geometry("rob BVHModel<OBBRSS>",
266
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 objToGeom<OBBRSS>((path / "rob.obj").string())));
267
268
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 geoms.push_back(Geometry("env BVHModel<OBB>",
269
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 objToGeom<OBB>((path / "env.obj").string())));
270 // geoms.push_back(Geometry ("env BVHModel<RSS>" , objToGeom<RSS> ((path
271 // / "env.obj").string()))); geoms.push_back(Geometry ("env BVHModel<kIOS>"
272 // , objToGeom<kIOS> ((path / "env.obj").string())));
273
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 geoms.push_back(Geometry("env BVHModel<OBBRSS>",
274
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 objToGeom<OBBRSS>((path / "env.obj").string())));
275
276
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 printResultHeaders();
277
278 // collision
279
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 10 for (std::size_t i = 0; i < geoms.size(); ++i) {
280
2/2
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 9 times.
 54 for (std::size_t j = i; j < geoms.size(); ++j) {
281
3/4
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 4 times.
✓ Branch 8 taken 41 times.
 45 if (!supportedPair(geoms[i].o.get(), geoms[j].o.get())) continue;
282
1/2
✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
 41 Results results(Ntransform);
283
1/2
✓ Branch 5 taken 41 times.
✗ Branch 6 not taken.
 41 collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request,
284 results);
285
1/2
✓ Branch 3 taken 41 times.
✗ Branch 4 not taken.
 41 printResults(geoms[i], geoms[j], results);
286 41 }
287 }
288 1 }
289
290 1 return 0;
291 1 }
292