GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/mesh_loader/assimp.cpp Lines: 42 49 85.7 %
Date: 2024-02-09 12:57:42 Branches: 27 72 37.5 %

Line Branch Exec Source
1
/*
2
 * Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2019-2021 CNRS - LAAS, INRIA
5
 *  All rights reserved.
6
 *
7
 *  Redistribution and use in source and binary forms, with or without
8
 *  modification, are permitted provided that the following conditions
9
 *  are met:
10
 *
11
 *   * Redistributions of source code must retain the above copyright
12
 *     notice, this list of conditions and the following disclaimer.
13
 *   * Redistributions in binary form must reproduce the above
14
 *     copyright notice, this list of conditions and the following
15
 *     disclaimer in the documentation and/or other materials provided
16
 *     with the distribution.
17
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
18
 *     contributors may be used to endorse or promote products derived
19
 *     from this software without specific prior written permission.
20
 *
21
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
 *  POSSIBILITY OF SUCH DAMAGE.
33
 */
34
35
#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
36
#define nullptr NULL
37
#endif
38
#include <hpp/fcl/mesh_loader/assimp.h>
39
40
// Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted
41
// https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for
42
// current version of hpp-fcl.
43
#include <assimp/defs.h>
44
#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && \
45
    defined(AI_NO_EXCEPT)
46
#undef AI_NO_EXCEPT
47
#define AI_NO_EXCEPT
48
#endif
49
50
#include <assimp/DefaultLogger.hpp>
51
#include <assimp/IOStream.hpp>
52
#include <assimp/IOSystem.hpp>
53
#include <assimp/Importer.hpp>
54
#include <assimp/postprocess.h>
55
#include <assimp/scene.h>
56
57
namespace hpp {
58
namespace fcl {
59
60
namespace internal {
61
62
159
Loader::Loader() : importer(new Assimp::Importer()) {
63
  // set list of ignored parameters (parameters used for rendering)
64
159
  importer->SetPropertyInteger(
65
      AI_CONFIG_PP_RVC_FLAGS,
66
      aiComponent_TANGENTS_AND_BITANGENTS | aiComponent_COLORS |
67
          aiComponent_BONEWEIGHTS | aiComponent_ANIMATIONS |
68
          aiComponent_LIGHTS | aiComponent_CAMERAS | aiComponent_TEXTURES |
69
          aiComponent_TEXCOORDS | aiComponent_MATERIALS | aiComponent_NORMALS);
70
71
  // remove LINES and POINTS
72
159
  importer->SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE,
73
                               aiPrimitiveType_LINE | aiPrimitiveType_POINT);
74
159
}
75
76
318
Loader::~Loader() {
77

159
  if (importer) delete importer;
78
159
}
79
80
159
void Loader::load(const std::string& resource_path) {
81
159
  scene = importer->ReadFile(
82
      resource_path.c_str(),
83
      aiProcess_SortByPType | aiProcess_Triangulate |
84
          aiProcess_RemoveComponent | aiProcess_ImproveCacheLocality |
85
          aiProcess_FindDegenerates | aiProcess_JoinIdenticalVertices);
86
87
159
  if (!scene) {
88
    const std::string exception_message(
89
        std::string("Could not load resource ") + resource_path +
90
        std::string("\n") + importer->GetErrorString() + std::string("\n") +
91
        "Hint: the mesh directory may be wrong.");
92
    throw std::invalid_argument(exception_message);
93
  }
94
95
159
  if (!scene->HasMeshes())
96
    throw std::invalid_argument(std::string("No meshes found in file ") +
97
                                resource_path);
98
159
}
99
100
/**
101
 * @brief      Recursive procedure for building a mesh
102
 *
103
 * @param[in]  scale           Scale to apply when reading the ressource
104
 * @param[in]  scene           Pointer to the assimp scene
105
 * @param[in]  node            Current node of the scene
106
 * @param[in]  vertices_offset Current number of vertices in the model
107
 * @param      tv              Triangles and Vertices of the mesh submodels
108
 */
109
604
unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene,
110
                          const aiNode* node, unsigned vertices_offset,
111
                          TriangleAndVertices& tv) {
112
604
  if (!node) return 0;
113
114
604
  aiMatrix4x4 transform = node->mTransformation;
115
604
  aiNode* pnode = node->mParent;
116
1049
  while (pnode) {
117
    // Don't convert to y-up orientation, which is what the root node in
118
    // Assimp does
119
445
    if (pnode->mParent != NULL) {
120
      transform = pnode->mTransformation * transform;
121
    }
122
445
    pnode = pnode->mParent;
123
  }
124
125
604
  unsigned nbVertices = 0;
126
1037
  for (uint32_t i = 0; i < node->mNumMeshes; i++) {
127
433
    aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
128
129
    // Add the vertices
130
196784
    for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) {
131
196351
      aiVector3D p = input_mesh->mVertices[j];
132
196351
      p *= transform;
133
196351
      tv.vertices_.push_back(
134


392702
          fcl::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2]));
135
    }
136
137
    // add the indices
138
220480
    for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
139
220047
      aiFace& face = input_mesh->mFaces[j];
140
220047
      assert(face.mNumIndices == 3 && "The size of the face is not valid.");
141
220047
      tv.triangles_.push_back(
142
220047
          fcl::Triangle(vertices_offset + face.mIndices[0],
143
220047
                        vertices_offset + face.mIndices[1],
144
220047
                        vertices_offset + face.mIndices[2]));
145
    }
146
147
433
    nbVertices += input_mesh->mNumVertices;
148
  }
149
150
1049
  for (uint32_t i = 0; i < node->mNumChildren; ++i) {
151
445
    nbVertices +=
152
445
        recurseBuildMesh(scale, scene, node->mChildren[i], nbVertices, tv);
153
  }
154
155
604
  return nbVertices;
156
}
157
158
159
void buildMesh(const fcl::Vec3f& scale, const aiScene* scene,
159
               unsigned vertices_offset, TriangleAndVertices& tv) {
160
159
  recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv);
161
159
}
162
163
}  // namespace internal
164
}  // namespace fcl
165
}  // namespace hpp