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