| 272 | } |
| 273 | |
| 274 | void OcTreeDrawer::initCubeTemplate(const octomath::Pose6D& origin, |
| 275 | std::vector<octomath::Vector3>& cube_template) { |
| 276 | cube_template.clear(); |
| 277 | cube_template.reserve(24); |
| 278 | |
| 279 | cube_template.push_back(octomath::Vector3( 1, 1,-1)); |
| 280 | cube_template.push_back(octomath::Vector3( 1,-1,-1)); |
| 281 | cube_template.push_back(octomath::Vector3( 1, 1,-1)); |
| 282 | cube_template.push_back(octomath::Vector3(-1, 1,-1)); |
| 283 | cube_template.push_back(octomath::Vector3( 1, 1,-1)); |
| 284 | cube_template.push_back(octomath::Vector3( 1, 1, 1)); |
| 285 | |
| 286 | cube_template.push_back(octomath::Vector3(-1, 1,-1)); |
| 287 | cube_template.push_back(octomath::Vector3(-1,-1,-1)); |
| 288 | cube_template.push_back(octomath::Vector3( 1, 1, 1)); |
| 289 | cube_template.push_back(octomath::Vector3(-1, 1, 1)); |
| 290 | cube_template.push_back(octomath::Vector3( 1,-1,-1)); |
| 291 | cube_template.push_back(octomath::Vector3( 1,-1, 1)); |
| 292 | |
| 293 | cube_template.push_back(octomath::Vector3(-1, 1, 1)); |
| 294 | cube_template.push_back(octomath::Vector3(-1,-1, 1)); |
| 295 | cube_template.push_back(octomath::Vector3( 1,-1, 1)); |
| 296 | cube_template.push_back(octomath::Vector3(-1,-1, 1)); |
| 297 | cube_template.push_back(octomath::Vector3(-1,-1,-1)); |
| 298 | cube_template.push_back(octomath::Vector3(-1,-1, 1)); |
| 299 | |
| 300 | cube_template.push_back(octomath::Vector3( 1, 1, 1)); |
| 301 | cube_template.push_back(octomath::Vector3( 1,-1, 1)); |
| 302 | cube_template.push_back(octomath::Vector3( 1,-1,-1)); |
| 303 | cube_template.push_back(octomath::Vector3(-1,-1,-1)); |
| 304 | cube_template.push_back(octomath::Vector3(-1, 1,-1)); |
| 305 | cube_template.push_back(octomath::Vector3(-1, 1, 1)); |
| 306 | } |
| 307 | |
| 308 | unsigned int OcTreeDrawer::generateCube(const octomap::OcTreeVolume& v, |
| 309 | const std::vector<octomath::Vector3>& cube_template, |