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 |
|
|
|