assimpUtil.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include "omplapp/geometry/detail/assimpUtil.h"
14 #include <ompl/util/Console.h>
15 #include <limits>
16 
17 void ompl::app::scene::inferBounds(base::RealVectorBounds &bounds, const std::vector<aiVector3D> &vertices, double multiply, double add)
18 {
19  double minX = std::numeric_limits<double>::infinity();
20  double minY = minX;
21  double minZ = minX;
22  double maxX = -minX;
23  double maxY = maxX;
24  double maxZ = maxX;
25  for (const auto & vertex : vertices)
26  {
27  if (minX > vertex.x) minX = vertex.x;
28  if (maxX < vertex.x) maxX = vertex.x;
29  if (minY > vertex.y) minY = vertex.y;
30  if (maxY < vertex.y) maxY = vertex.y;
31  if (minZ > vertex.z) minZ = vertex.z;
32  if (maxZ < vertex.z) maxZ = vertex.z;
33  }
34 
35  multiply -= 1.0;
36  if (multiply < 0.0)
37  {
38  multiply = 0.0;
39  OMPL_WARN("The multiplicative factor in the bounds computation process should be larger than 1.0");
40  }
41  if (add < 0.0)
42  {
43  add = 0.0;
44  OMPL_WARN("The additive factor in the bounds computation process should be larger than 0.0");
45  }
46 
47  double dx = (maxX - minX) * multiply + add;
48  double dy = (maxY - minY) * multiply + add;
49  double dz = (maxZ - minZ) * multiply + add;
50  bounds.low[0] = minX - dx; bounds.low[1] = minY - dy; bounds.low[2] = minZ - dz;
51  bounds.high[0] = maxX + dx; bounds.high[1] = maxY + dy; bounds.high[2] = maxZ + dz;
52 }
53 
54 
55 namespace ompl
56 {
57  namespace app
58  {
59 
60  namespace scene
61  {
62 
63  void extractVerticesAux(const aiScene *scene, const aiNode *node, aiMatrix4x4 transform,
64  std::vector<aiVector3D> &vertices)
65  {
66  transform *= node->mTransformation;
67  for (unsigned int i = 0 ; i < node->mNumMeshes; ++i)
68  {
69  const aiMesh* a = scene->mMeshes[node->mMeshes[i]];
70  for (unsigned int i = 0 ; i < a->mNumVertices ; ++i)
71  vertices.push_back(transform * a->mVertices[i]);
72  }
73 
74  for (unsigned int n = 0; n < node->mNumChildren; ++n)
75  extractVerticesAux(scene, node->mChildren[n], transform, vertices);
76  }
77 
78  void extractTrianglesAux(const aiScene *scene, const aiNode *node, aiMatrix4x4 transform,
79  std::vector<aiVector3D> &triangles)
80  {
81  transform *= node->mTransformation;
82  for (unsigned int i = 0 ; i < node->mNumMeshes; ++i)
83  {
84  const aiMesh* a = scene->mMeshes[node->mMeshes[i]];
85  for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
86  if (a->mFaces[i].mNumIndices == 3)
87  {
88  triangles.push_back(transform * a->mVertices[a->mFaces[i].mIndices[0]]);
89  triangles.push_back(transform * a->mVertices[a->mFaces[i].mIndices[1]]);
90  triangles.push_back(transform * a->mVertices[a->mFaces[i].mIndices[2]]);
91  }
92  }
93 
94  for (unsigned int n = 0; n < node->mNumChildren; ++n)
95  extractTrianglesAux(scene, node->mChildren[n], transform, triangles);
96  }
97  }
98  }
99 }
100 
101 
102 void ompl::app::scene::extractVertices(const aiScene *scene, std::vector<aiVector3D> &vertices)
103 {
104  vertices.clear();
105  if (scene && scene->HasMeshes())
106  extractVerticesAux(scene, scene->mRootNode, aiMatrix4x4(), vertices);
107 }
108 
109 void ompl::app::scene::sceneCenter(const aiScene *scene, aiVector3D &center)
110 {
111  std::vector<aiVector3D> vertices;
112  extractVertices(scene, vertices);
113  center.Set(0, 0, 0);
114  for (auto & vertex : vertices)
115  center += vertex;
116  center /= (float)vertices.size();
117 }
118 
119 void ompl::app::scene::extractTriangles(const aiScene *scene, std::vector<aiVector3D> &triangles)
120 {
121  triangles.clear();
122  if (scene && scene->HasMeshes())
123  extractTrianglesAux(scene, scene->mRootNode, aiMatrix4x4(), triangles);
124 }
125 
126 double ompl::app::scene::shortestEdge(const aiScene *scene)
127 {
128  std::vector<aiVector3D> triangles;
129  extractTriangles(scene, triangles);
130  double s = std::numeric_limits<double>::infinity();
131  for (unsigned int i = 0 ; i < triangles.size() / 3 ; ++i)
132  {
133  double d = (triangles[3 * i] - triangles[3 * i + 1]).Length();
134  if (d < s) s = d;
135  d = (triangles[3 * i] - triangles[3 * i + 2]).Length();
136  if (d < s) s = d;
137  d = (triangles[3 * i + 1] - triangles[3 * i + 2]).Length();
138  if (d < s) s = d;
139  }
140  return s;
141 }
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66