diff --git a/.github/workflows/publish_dartpy.yml b/.github/workflows/publish_dartpy.yml index 3f4d3c026c5b3..e23c0a7752069 100644 --- a/.github/workflows/publish_dartpy.yml +++ b/.github/workflows/publish_dartpy.yml @@ -98,8 +98,6 @@ jobs: uses: mozilla-actions/sccache-action@v0.0.9 with: disable_annotations: true - # Enable only when GitHub cache backend is available; otherwise skip. - use-gha-cache: ${{ env.ACTIONS_CACHE_URL != '' }} - name: Configure environment for compiler cache if: (matrix.skip-on-commit != true) || github.event_name == 'pull_request' || startsWith(github.ref, 'refs/tags/v') @@ -107,6 +105,9 @@ jobs: - name: Build wheel if: (matrix.skip-on-commit != true) || github.event_name == 'pull_request' || startsWith(github.ref, 'refs/tags/v') + env: + # Work around occasional sccache spawn failures on ubuntu-latest + Python 3.14 wheel builds. + DART_DISABLE_COMPILER_CACHE: ${{ (matrix.os == 'ubuntu-latest' && matrix.python-version == '314') && 'ON' || 'OFF' }} run: pixi run -e py${{ matrix.python-version }}-wheel wheel-build - name: Repair wheel diff --git a/AGENTS.md b/AGENTS.md index a619480703bdb..7de453ce78ead 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -5,7 +5,7 @@ This file is a pointer board for agents working in this repository. Keep it conc ## Read First - Architectural, build, and workflow expectations live in `docs/onboarding` (start with `docs/onboarding/ci-cd.md` and `docs/onboarding/build-system.md`). -- The day-to-day pixi workflow (install, config, build, test) is documented in `docs/onboarding/building.md`. +- The day-to-day pixi workflow (install, config, build, lint, test) is documented in `docs/onboarding/building.md` and `docs/onboarding/testing.md`. - Coding standards, formatting, and contribution flow are in `CONTRIBUTING.md`. - Feature‑specific notes belong beside the code (e.g., README in the component directory) or in `docs/`. - Unified model loading API (`dart::io`) is documented in `docs/onboarding/io-parsing.md`. diff --git a/cmake/DARTFindDependencies.cmake b/cmake/DARTFindDependencies.cmake index b0cc14594ec62..261997dfc198d 100644 --- a/cmake/DARTFindDependencies.cmake +++ b/cmake/DARTFindDependencies.cmake @@ -165,6 +165,15 @@ if(DART_BUILD_GUI) ${IMGUI_BACKEND_HEADERS} ) + if(WIN32 AND BUILD_SHARED_LIBS) + # ImGui does not export symbols by default; ensure an import library is + # generated so dart-gui can link against dart-imgui-lib on Windows. + set_target_properties( + ${imgui_library_name} + PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON + ) + endif() + # Configure include directories # Build tree: use fetched source directory # Install tree: use standard include paths (like system-installed imgui) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 3d9cadf420d18..bba40c6c89855 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -60,6 +60,8 @@ #include #include +#include + namespace dart { namespace collision { @@ -91,17 +93,15 @@ std::unique_ptr createBulletEllipsoidMesh( std::unique_ptr createBulletEllipsoidMultiSphere( const Eigen::Vector3d& radii); -std::unique_ptr createBulletCollisionShapeFromAssimpScene( - const Eigen::Vector3d& scale, const aiScene* scene); - -std::unique_ptr createBulletCollisionShapeFromAssimpMesh( - const aiMesh* mesh); +std::unique_ptr createBulletCollisionShapeFromTriMesh( + const Eigen::Vector3d& scale, + const std::shared_ptr>& mesh); template std::unique_ptr createBulletCollisionShapeFromHeightmap( const HeightmapShapeT* heightMap); -bool isConvex(const aiMesh* mesh, float threshold = 0.001); +bool isConvex(const math::TriMesh& mesh, float threshold = 0.001f); } // anonymous namespace @@ -649,17 +649,19 @@ BulletCollisionDetector::createBulletCollisionShape( std::move(bulletCollisionShape)); } else if (const auto shapeMesh = shape->as()) { const auto scale = shapeMesh->getScale(); - const auto mesh = shapeMesh->getMesh(); + const auto triMesh = shapeMesh->getTriMesh(); auto bulletCollisionShape - = createBulletCollisionShapeFromAssimpScene(scale, mesh); + = createBulletCollisionShapeFromTriMesh(scale, triMesh); return std::make_unique( std::move(bulletCollisionShape)); } else if (const auto softMeshShape = shape->as()) { - const auto mesh = softMeshShape->getAssimpMesh(); + const auto triMesh = softMeshShape->getTriMesh(); + const Eigen::Vector3d scale = Eigen::Vector3d::Ones(); - auto bulletCollisionShape = createBulletCollisionShapeFromAssimpMesh(mesh); + auto bulletCollisionShape + = createBulletCollisionShapeFromTriMesh(scale, triMesh); return std::make_unique( std::move(bulletCollisionShape)); @@ -1015,62 +1017,40 @@ std::unique_ptr createBulletEllipsoidMultiSphere( } //============================================================================== -std::unique_ptr createBulletCollisionShapeFromAssimpScene( - const Eigen::Vector3d& scale, const aiScene* scene) +std::unique_ptr createBulletCollisionShapeFromTriMesh( + const Eigen::Vector3d& scale, + const std::shared_ptr>& mesh) { - auto triMesh = new btTriangleMesh(); - - for (auto i = 0u; i < scene->mNumMeshes; ++i) { - for (auto j = 0u; j < scene->mMeshes[i]->mNumFaces; ++j) { - btVector3 vertices[3]; - for (auto k = 0u; k < 3; ++k) { - const aiVector3D& vertex - = scene->mMeshes[i] - ->mVertices[scene->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = btVector3( - vertex.x * scale[0], vertex.y * scale[1], vertex.z * scale[2]); - } - triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - const bool makeConvexMesh - = scene->mNumMeshes == 1 && isConvex(scene->mMeshes[0]); - if (makeConvexMesh) { - auto convexMeshShape = std::make_unique(triMesh); - convexMeshShape->setMargin(0.0f); - convexMeshShape->setUserPointer(triMesh); - return convexMeshShape; - } else { - auto gimpactMeshShape = std::make_unique(triMesh); - gimpactMeshShape->updateBound(); - gimpactMeshShape->setUserPointer(triMesh); - return gimpactMeshShape; + if (!mesh) { + return nullptr; } -} -//============================================================================== -std::unique_ptr createBulletCollisionShapeFromAssimpMesh( - const aiMesh* mesh) -{ auto triMesh = new btTriangleMesh(); - for (auto i = 0u; i < mesh->mNumFaces; ++i) { - btVector3 vertices[3]; - for (auto j = 0u; j < 3; ++j) { - const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; - vertices[j] = btVector3(vertex.x, vertex.y, vertex.z); + const auto& vertices = mesh->getVertices(); + const auto& triangles = mesh->getTriangles(); + + for (const auto& triangle : triangles) { + btVector3 btVertices[3]; + for (auto k = 0u; k < 3; ++k) { + const auto& vertex = vertices[triangle[k]]; + btVertices[k] = btVector3( + vertex.x() * scale[0], vertex.y() * scale[1], vertex.z() * scale[2]); } - triMesh->addTriangle(vertices[0], vertices[1], vertices[2]); + triMesh->addTriangle(btVertices[0], btVertices[1], btVertices[2]); } - const bool makeConvexMesh = isConvex(mesh); + const bool makeConvexMesh = isConvex(*mesh); + if (makeConvexMesh) { auto convexMeshShape = std::make_unique(triMesh); convexMeshShape->setMargin(0.0f); + convexMeshShape->setUserPointer(triMesh); return convexMeshShape; } else { auto gimpactMeshShape = std::make_unique(triMesh); gimpactMeshShape->updateBound(); + gimpactMeshShape->setUserPointer(triMesh); return gimpactMeshShape; } } @@ -1144,36 +1124,46 @@ std::unique_ptr createBulletCollisionShapeFromHeightmap( } //============================================================================== -bool isConvex(const aiMesh* mesh, float threshold) +bool isConvex(const math::TriMesh& mesh, float threshold) { - // Check whether all the other vertices on the mesh is on the internal side of - // the face, assuming that the direction of the normal of the face is pointing - // external side. + // Check whether all the other vertices on the mesh are on the internal side + // of the face, assuming that the direction of the normal of the face is + // pointing to the external side. // // Reference: https://stackoverflow.com/a/40056279/3122234 + const auto& points = mesh.getVertices(); + const auto& triangles = mesh.getTriangles(); + + if (points.empty() || triangles.empty()) { + return false; + } - const auto points = mesh->mVertices; btVector3 vertices[3]; - for (auto i = 0u; i < mesh->mNumFaces; ++i) { + for (const auto& triangle : triangles) { for (auto j = 0u; j < 3; ++j) { - const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; - vertices[j] = btVector3(vertex.x, vertex.y, vertex.z); + const auto& vertex = points[triangle[j]]; + vertices[j] = btVector3(vertex.x(), vertex.y(), vertex.z()); } const btVector3& A = vertices[0]; const btVector3 B = vertices[1] - A; const btVector3 C = vertices[2] - A; - const btVector3 BCNorm = B.cross(C).normalized(); + const btVector3 BCNormRaw = B.cross(C); + const btScalar normSquared = BCNormRaw.length2(); + if (normSquared <= btScalar(0.0)) { + continue; + } + const btVector3 BCNorm = BCNormRaw / btSqrt(normSquared); - const float checkPoint - = btVector3( - points[0].x - A.x(), points[0].y - A.y(), points[0].z - A.z()) - .dot(BCNorm); + const float checkPoint = btVector3( + points[0].x() - A.x(), + points[0].y() - A.y(), + points[0].z() - A.z()) + .dot(BCNorm); - for (auto j = 0u; j < mesh->mNumVertices; ++j) { - float dist - = btVector3( - points[j].x - A.x(), points[j].y - A.y(), points[j].z - A.z()) + for (const auto& point : points) { + const float dist + = btVector3(point.x() - A.x(), point.y() - A.y(), point.z() - A.z()) .dot(BCNorm); if ((std::abs(checkPoint) > threshold) && (std::abs(dist) > threshold) && (checkPoint * dist < 0.0f)) { @@ -1181,6 +1171,7 @@ bool isConvex(const aiMesh* mesh, float threshold) } } } + return true; } diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 8a4a90fb51103..049cac602e709 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -38,8 +38,6 @@ #include #include -#include - #include #include diff --git a/dart/collision/fcl/CollisionShapes.hpp b/dart/collision/fcl/CollisionShapes.hpp index 268819ab9fd56..89631a2aceb0b 100644 --- a/dart/collision/fcl/CollisionShapes.hpp +++ b/dart/collision/fcl/CollisionShapes.hpp @@ -39,43 +39,13 @@ #include -#include - #include namespace dart { namespace collision { -template -::fcl::BVHModel* createMesh( - double _scaleX, - double _scaleY, - double _scaleZ, - const aiScene* _mesh, - const dart::collision::fcl::Transform3& _transform) -{ - DART_ASSERT(_mesh); - ::fcl::BVHModel* model = new ::fcl::BVHModel; - model->beginModel(); - - for (unsigned int i = 0; i < _mesh->mNumMeshes; i++) { - for (unsigned int j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) { - dart::collision::fcl::Vector3 vertices[3]; - for (unsigned int k = 0; k < 3; k++) { - const aiVector3D& vertex - = _mesh->mMeshes[i] - ->mVertices[_mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = dart::collision::fcl::Vector3( - vertex.x * _scaleX, vertex.y * _scaleY, vertex.z * _scaleZ); - vertices[k] = dart::collision::fcl::transform(_transform, vertices[k]); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); - } - } - - model->endModel(); - return model; -} +// Deprecated aiScene-based createMesh removed - use TriMesh-based alternatives +// in FCLCollisionDetector instead template ::fcl::BVHModel* createEllipsoid( diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 806b8efbf4daf..84ab49cb33293 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -55,8 +55,6 @@ #include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/VoxelGridShape.hpp" -#include - #include #include #include @@ -562,48 +560,56 @@ ::fcl::BVHModel* createPyramid( //============================================================================== template -::fcl::BVHModel* createMesh( - double _scaleX, double _scaleY, double _scaleZ, const aiScene* _mesh) +::fcl::BVHModel* createMeshFromTriMesh( + double _scaleX, + double _scaleY, + double _scaleZ, + const std::shared_ptr>& _triMesh) { - // Create FCL mesh from Assimp mesh + // Create FCL mesh from TriMesh - DART_ASSERT(_mesh); + DART_ASSERT(_triMesh); ::fcl::BVHModel* model = new ::fcl::BVHModel; model->beginModel(); - for (std::size_t i = 0; i < _mesh->mNumMeshes; i++) { - for (std::size_t j = 0; j < _mesh->mMeshes[i]->mNumFaces; j++) { - fcl::Vector3 vertices[3]; - for (std::size_t k = 0; k < 3; k++) { - const aiVector3D& vertex - = _mesh->mMeshes[i] - ->mVertices[_mesh->mMeshes[i]->mFaces[j].mIndices[k]]; - vertices[k] = fcl::Vector3( - vertex.x * _scaleX, vertex.y * _scaleY, vertex.z * _scaleZ); - } - model->addTriangle(vertices[0], vertices[1], vertices[2]); + + const auto& vertices = _triMesh->getVertices(); + const auto& triangles = _triMesh->getTriangles(); + + for (const auto& triangle : triangles) { + fcl::Vector3 fclVertices[3]; + for (std::size_t i = 0; i < 3; i++) { + const auto& vertex = vertices[triangle[i]]; + fclVertices[i] = fcl::Vector3( + vertex.x() * _scaleX, vertex.y() * _scaleY, vertex.z() * _scaleZ); } + model->addTriangle(fclVertices[0], fclVertices[1], fclVertices[2]); } + model->endModel(); return model; } //============================================================================== template -::fcl::BVHModel* createSoftMesh(const aiMesh* _mesh) +::fcl::BVHModel* createSoftMesh( + const std::shared_ptr>& triMesh) { - // Create FCL mesh from Assimp mesh + // Create FCL mesh from TriMesh - DART_ASSERT(_mesh); + DART_ASSERT(triMesh); ::fcl::BVHModel* model = new ::fcl::BVHModel; model->beginModel(); - for (std::size_t i = 0; i < _mesh->mNumFaces; i++) { - fcl::Vector3 vertices[3]; + const auto& vertices = triMesh->getVertices(); + const auto& triangles = triMesh->getTriangles(); + + for (const auto& triangle : triangles) { + fcl::Vector3 fclVertices[3]; for (std::size_t j = 0; j < 3; j++) { - const aiVector3D& vertex = _mesh->mVertices[_mesh->mFaces[i].mIndices[j]]; - vertices[j] = fcl::Vector3(vertex.x, vertex.y, vertex.z); + const auto& vertex = vertices[triangle[j]]; + fclVertices[j] = fcl::Vector3(vertex.x(), vertex.y(), vertex.z()); } - model->addTriangle(vertices[0], vertices[1], vertices[2]); + model->addTriangle(fclVertices[0], fclVertices[1], fclVertices[2]); } model->endModel(); @@ -1021,16 +1027,18 @@ FCLCollisionDetector::createFCLCollisionGeometry( auto shapeMesh = static_cast(shape.get()); const Eigen::Vector3d& scale = shapeMesh->getScale(); - auto aiScene = shapeMesh->getMesh(); - geom = createMesh(scale[0], scale[1], scale[2], aiScene); + auto triMesh = shapeMesh->getTriMesh(); + + geom = createMeshFromTriMesh( + scale[0], scale[1], scale[2], triMesh); } else if (SoftMeshShape::getStaticType() == shapeType) { DART_ASSERT(dynamic_cast(shape.get())); auto softMeshShape = static_cast(shape.get()); - auto aiMesh = softMeshShape->getAssimpMesh(); + auto triMesh = softMeshShape->getTriMesh(); - geom = createSoftMesh(aiMesh); + geom = createSoftMesh(triMesh); } #if DART_HAVE_OCTOMAP else if (VoxelGridShape::getStaticType() == shapeType) { diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index dbe1ce3a56d38..42febfd5e1e72 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -99,10 +99,13 @@ void FCLCollisionObject::updateEngineData() DART_ASSERT(dynamic_cast(shape)); auto softMeshShape = static_cast(shape); - const aiMesh* mesh = softMeshShape->getAssimpMesh(); const_cast(softMeshShape)->update(); // TODO(JS): update function be called by somewhere out of here. + const auto triMesh = softMeshShape->getTriMesh(); + const auto& vertices = triMesh->getVertices(); + const auto& triangles = triMesh->getTriangles(); + auto collGeom = const_cast( mFCLCollisionObject->collisionGeometry().get()); DART_ASSERT( @@ -111,14 +114,14 @@ void FCLCollisionObject::updateEngineData() = static_cast<::fcl::BVHModel*>(collGeom); bvhModel->beginUpdateModel(); - for (auto i = 0u; i < mesh->mNumFaces; ++i) { - dart::collision::fcl::Vector3 vertices[3]; + for (const auto& triangle : triangles) { + dart::collision::fcl::Vector3 fclVertices[3]; for (auto j = 0u; j < 3; ++j) { - const auto& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]]; - vertices[j] - = dart::collision::fcl::Vector3(vertex.x, vertex.y, vertex.z); + const auto& vertex = vertices[triangle[j]]; + fclVertices[j] + = dart::collision::fcl::Vector3(vertex.x(), vertex.y(), vertex.z()); } - bvhModel->updateTriangle(vertices[0], vertices[1], vertices[2]); + bvhModel->updateTriangle(fclVertices[0], fclVertices[1], fclVertices[2]); } bvhModel->endUpdateModel(); } diff --git a/dart/collision/ode/OdeCollisionObject.cpp b/dart/collision/ode/OdeCollisionObject.cpp index b59487a17d772..067639dc2dae4 100644 --- a/dart/collision/ode/OdeCollisionObject.cpp +++ b/dart/collision/ode/OdeCollisionObject.cpp @@ -210,9 +210,9 @@ detail::OdeGeom* createOdeGeom( geom = new detail::OdePlane(collObj, normal, offset); } else if (const auto shapeMesh = shape->as()) { const Eigen::Vector3d& scale = shapeMesh->getScale(); - auto aiScene = shapeMesh->getMesh(); + auto triMesh = shapeMesh->getTriMesh(); - geom = new detail::OdeMesh(collObj, aiScene, scale); + geom = new detail::OdeMesh(collObj, triMesh, scale); } else if (const auto heightMap = shape->as()) { geom = new detail::OdeHeightmapf(collObj, heightMap); } else if (const auto heightMap = shape->as()) { diff --git a/dart/collision/ode/detail/OdeMesh.cpp b/dart/collision/ode/detail/OdeMesh.cpp index 8fa27875dd305..28dd3e78111c4 100644 --- a/dart/collision/ode/detail/OdeMesh.cpp +++ b/dart/collision/ode/detail/OdeMesh.cpp @@ -41,17 +41,21 @@ namespace detail { //============================================================================== OdeMesh::OdeMesh( const OdeCollisionObject* parent, - const aiScene* scene, + const std::shared_ptr>& mesh, const Eigen::Vector3d& scale) : OdeGeom(parent), mOdeTriMeshDataId(nullptr) { // Fill vertices, normals, and indices in the ODE friendly data structures. - fillArrays(scene, scale); + fillArraysFromTriMesh(mesh, scale); /// This will hold the vertex data of the triangle mesh if (!mOdeTriMeshDataId) mOdeTriMeshDataId = dGeomTriMeshDataCreate(); + const double* normalData = nullptr; + if (!mNormals.empty() && mNormals.size() == mVertices.size()) + normalData = mNormals.data(); + // Build the ODE triangle mesh dGeomTriMeshDataBuildDouble1( mOdeTriMeshDataId, @@ -61,7 +65,7 @@ OdeMesh::OdeMesh( mIndices.data(), static_cast(mIndices.size()), 3 * sizeof(int), - mNormals.data()); + normalData); mGeomId = dCreateTriMesh(0, mOdeTriMeshDataId, nullptr, nullptr, nullptr); } @@ -82,54 +86,44 @@ void OdeMesh::updateEngineData() } //============================================================================== -void OdeMesh::fillArrays(const aiScene* scene, const Eigen::Vector3d& scale) +void OdeMesh::fillArraysFromTriMesh( + const std::shared_ptr>& mesh, + const Eigen::Vector3d& scale) { mVertices.clear(); mNormals.clear(); mIndices.clear(); - // Count the total numbers of vertices and indices. - auto mNumVertices = 0u; - auto mNumIndices = 0u; - for (auto i = 0u; i < scene->mNumMeshes; ++i) { - const auto mesh = scene->mMeshes[i]; + if (!mesh) { + return; + } + + const auto& vertices = mesh->getVertices(); + const auto& triangles = mesh->getTriangles(); + const auto& normals = mesh->getVertexNormals(); - mNumVertices += mesh->mNumVertices; - mNumIndices += mesh->mNumFaces; + // Fill vertices + mVertices.reserve(vertices.size() * 3); + for (const auto& vertex : vertices) { + mVertices.push_back(vertex.x() * scale.x()); + mVertices.push_back(vertex.y() * scale.y()); + mVertices.push_back(vertex.z() * scale.z()); } - mNumVertices *= 3u; - mNumIndices *= 3u; - // The number of indices of each face is always 3 because we use the assimp - // option `aiProcess_Triangulate` when loading meshes. - - mVertices.resize(mNumVertices); - mNormals.resize(mNumVertices); - mIndices.resize(mNumIndices); - - auto vertexIndex = 0u; - auto indexIndex = 0u; - auto offset = 0u; - for (auto i = 0u; i < scene->mNumMeshes; ++i) { - const auto mesh = scene->mMeshes[i]; - - for (auto j = 0u; j < mesh->mNumVertices; ++j) { - mVertices[vertexIndex] = mesh->mVertices[j].x * scale.x(); - mNormals[vertexIndex++] = mesh->mNormals[j].x; - - mVertices[vertexIndex] = mesh->mVertices[j].y * scale.y(); - mNormals[vertexIndex++] = mesh->mNormals[j].y; - - mVertices[vertexIndex] = mesh->mVertices[j].z * scale.z(); - mNormals[vertexIndex++] = mesh->mNormals[j].z; - } - - for (auto j = 0u; j < mesh->mNumFaces; ++j) { - mIndices[indexIndex++] = mesh->mFaces[j].mIndices[0] + offset; - mIndices[indexIndex++] = mesh->mFaces[j].mIndices[1] + offset; - mIndices[indexIndex++] = mesh->mFaces[j].mIndices[2] + offset; - } - - offset += mesh->mNumVertices; + + // Fill normals + mNormals.reserve(normals.size() * 3); + for (const auto& normal : normals) { + mNormals.push_back(normal.x()); + mNormals.push_back(normal.y()); + mNormals.push_back(normal.z()); + } + + // Fill indices + mIndices.reserve(triangles.size() * 3); + for (const auto& triangle : triangles) { + mIndices.push_back(static_cast(triangle.x())); + mIndices.push_back(static_cast(triangle.y())); + mIndices.push_back(static_cast(triangle.z())); } } diff --git a/dart/collision/ode/detail/OdeMesh.hpp b/dart/collision/ode/detail/OdeMesh.hpp index 208854dfa0e3c..77c4c35ea9d27 100644 --- a/dart/collision/ode/detail/OdeMesh.hpp +++ b/dart/collision/ode/detail/OdeMesh.hpp @@ -35,9 +35,12 @@ #include -#include +#include + #include +#include + namespace dart { namespace collision { namespace detail { @@ -45,10 +48,10 @@ namespace detail { class OdeMesh : public OdeGeom { public: - /// Constructor + /// Constructor using TriMesh (preferred) OdeMesh( const OdeCollisionObject* parent, - const aiScene* scene, + const std::shared_ptr>& mesh, const Eigen::Vector3d& scale = Eigen::Vector3d::Ones()); /// Destructor @@ -58,8 +61,8 @@ class OdeMesh : public OdeGeom void updateEngineData() override; private: - void fillArrays( - const aiScene* scene, + void fillArraysFromTriMesh( + const std::shared_ptr>& mesh, const Eigen::Vector3d& scale = Eigen::Vector3d::Ones()); private: diff --git a/dart/dynamics/ArrowShape.cpp b/dart/dynamics/ArrowShape.cpp index c9cbb13073da2..b4671acf9aaaf 100644 --- a/dart/dynamics/ArrowShape.cpp +++ b/dart/dynamics/ArrowShape.cpp @@ -55,7 +55,12 @@ ArrowShape::Properties::Properties( } //============================================================================== -ArrowShape::ArrowShape() : MeshShape(Eigen::Vector3d::Ones(), nullptr) {} +ArrowShape::ArrowShape() + : MeshShape( + Eigen::Vector3d::Ones(), std::make_shared>()), + mResolution(10) +{ +} //============================================================================== ArrowShape::ArrowShape( @@ -64,17 +69,25 @@ ArrowShape::ArrowShape( const Properties& _properties, const Eigen::Vector4d& _color, std::size_t _resolution) - : MeshShape(Eigen::Vector3d::Ones(), nullptr), + : MeshShape( + Eigen::Vector3d::Ones(), std::make_shared>()), mTail(_tail), mHead(_head), - mProperties(_properties) + mProperties(_properties), + mResolution(_resolution) { instantiate(_resolution); configureArrow(mTail, mHead, mProperties); - setColorMode(MeshShape::COLOR_INDEX); + setColorMode(MeshShape::MATERIAL_COLOR); notifyColorUpdated(_color); } +//============================================================================== +ArrowShape::~ArrowShape() +{ + // No manual cleanup needed - TriMesh is managed by shared_ptr +} + //============================================================================== void ArrowShape::setPositions( const Eigen::Vector3d& _tail, const Eigen::Vector3d& _head) @@ -103,69 +116,26 @@ void ArrowShape::setProperties(const Properties& _properties) //============================================================================== void ArrowShape::notifyColorUpdated(const Eigen::Vector4d& _color) { - for (std::size_t i = 0; i < mMesh->mNumMeshes; ++i) { - aiMesh* mesh = mMesh->mMeshes[i]; - for (std::size_t j = 0; j < mesh->mNumVertices; ++j) { - mesh->mColors[0][j] - = aiColor4D(_color[0], _color[1], _color[2], _color[3]); - } - } -} - -//============================================================================== -const ArrowShape::Properties& ArrowShape::getProperties() const -{ - return mProperties; -} - -//============================================================================== -static void constructArrowTip( - aiMesh* mesh, - double base, - double tip, - const ArrowShape::Properties& properties) -{ - std::size_t resolution = (mesh->mNumVertices - 1) / 2; - for (std::size_t i = 0; i < resolution; ++i) { - double theta = (double)(i) / (double)(resolution)*2 * math::pi; - - double R = properties.mRadius; - double x = R * cos(theta); - double y = R * sin(theta); - double z = base; - mesh->mVertices[2 * i].Set(x, y, z); + if (mMaterials.empty()) + mMaterials.emplace_back(); - if (base != tip) { - x *= properties.mHeadRadiusScale; - y *= properties.mHeadRadiusScale; - } + auto& material = mMaterials.front(); - mesh->mVertices[2 * i + 1].Set(x, y, z); - } + const Eigen::Vector4f color = _color.cast(); + material.diffuse = color; + material.ambient = Eigen::Vector4f( + 0.2f * color.x(), 0.2f * color.y(), 0.2f * color.z(), color.w()); + material.specular = Eigen::Vector4f(0.0f, 0.0f, 0.0f, color.w()); + material.emissive = Eigen::Vector4f(0.0f, 0.0f, 0.0f, color.w()); + material.shininess = 0.0f; - mesh->mVertices[mesh->mNumVertices - 1].Set(0, 0, tip); + incrementVersion(); } //============================================================================== -static void constructArrowBody( - aiMesh* mesh, - double z1, - double z2, - const ArrowShape::Properties& properties) +const ArrowShape::Properties& ArrowShape::getProperties() const { - std::size_t resolution = mesh->mNumVertices / 2; - for (std::size_t i = 0; i < resolution; ++i) { - double theta = (double)(i) / (double)(resolution)*2 * math::pi; - - double R = properties.mRadius; - double x = R * cos(theta); - double y = R * sin(theta); - double z = z1; - mesh->mVertices[2 * i].Set(x, y, z); - - z = z2; - mesh->mVertices[2 * i + 1].Set(x, y, z); - } + return mProperties; } //============================================================================== @@ -196,25 +166,149 @@ void ArrowShape::configureArrow( double headLength = mProperties.mHeadLengthScale * length; headLength = std::min(maxHeadLength, std::max(minHeadLength, headLength)); - // construct the tail - if (mProperties.mDoubleArrow) { - constructArrowTip(mMesh->mMeshes[0], headLength, 0, mProperties); - } else { - constructArrowTip(mMesh->mMeshes[0], 0, 0, mProperties); + // Regenerate the entire mesh with new parameters + if (mCachedAiScene) { + aiReleaseImport(mCachedAiScene); + mCachedAiScene = nullptr; } + mTriMesh = std::make_shared>(); - // construct the main body + const double pi = math::pi; + const std::size_t resolution = mResolution; + + // Calculate z-coordinates for different parts + double tailTipZ = 0.0; + double tailBaseZ = mProperties.mDoubleArrow ? headLength : 0.0; + double bodyStartZ = tailBaseZ; + double bodyEndZ = length - headLength; + double headBaseZ = bodyEndZ; + double headTipZ = length; + + // Calculate total number of vertices and triangles + std::size_t tailVertices + = mProperties.mDoubleArrow ? (2 * resolution + 1) : 0; + std::size_t bodyVertices = 2 * resolution; + std::size_t headVertices = 2 * resolution + 1; + std::size_t totalVertices = tailVertices + bodyVertices + headVertices; + + std::size_t tailTriangles = mProperties.mDoubleArrow ? (3 * resolution) : 0; + std::size_t bodyTriangles = 2 * resolution; + std::size_t headTriangles = 3 * resolution; + std::size_t totalTriangles = tailTriangles + bodyTriangles + headTriangles; + + mTriMesh->reserveVertices(totalVertices); + mTriMesh->reserveTriangles(totalTriangles); + + std::size_t vertexOffset = 0; + + // Generate tail (if double arrow) if (mProperties.mDoubleArrow) { - constructArrowBody( - mMesh->mMeshes[1], headLength, length - headLength, mProperties); - } else { - constructArrowBody(mMesh->mMeshes[1], 0, length - headLength, mProperties); + // Tail cone vertices + for (std::size_t i = 0; i < resolution; ++i) { + double theta = (double)(i) / (double)(resolution)*2.0 * pi; + double x = mProperties.mRadius * cos(theta); + double y = mProperties.mRadius * sin(theta); + + // Back of tail cone (at tip) + mTriMesh->addVertex(Eigen::Vector3d(x, y, tailTipZ)); + // Base of tail cone + mTriMesh->addVertex(Eigen::Vector3d( + x * mProperties.mHeadRadiusScale, + y * mProperties.mHeadRadiusScale, + tailBaseZ)); + } + // Tail tip vertex + mTriMesh->addVertex(Eigen::Vector3d(0, 0, tailTipZ)); + + // Tail triangles + for (std::size_t i = 0; i < resolution; ++i) { + std::size_t next = (i + 1) % resolution; + + // Back face triangles + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i, + vertexOffset + 2 * i + 1, + vertexOffset + 2 * next + 1)); + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i, + vertexOffset + 2 * next + 1, + vertexOffset + 2 * next)); + + // Cone tip triangles + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i + 1, + vertexOffset + 2 * resolution, + vertexOffset + 2 * next + 1)); + } + + vertexOffset += tailVertices; + } + + // Generate body (cylinder) + for (std::size_t i = 0; i < resolution; ++i) { + double theta = (double)(i) / (double)(resolution)*2.0 * pi; + double x = mProperties.mRadius * cos(theta); + double y = mProperties.mRadius * sin(theta); + + mTriMesh->addVertex(Eigen::Vector3d(x, y, bodyStartZ)); + mTriMesh->addVertex(Eigen::Vector3d(x, y, bodyEndZ)); + } + + // Body triangles + for (std::size_t i = 0; i < resolution; ++i) { + std::size_t next = (i + 1) % resolution; + + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i, + vertexOffset + 2 * next + 1, + vertexOffset + 2 * i + 1)); + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i, + vertexOffset + 2 * next, + vertexOffset + 2 * next + 1)); + } + + vertexOffset += bodyVertices; + + // Generate head (cone) + for (std::size_t i = 0; i < resolution; ++i) { + double theta = (double)(i) / (double)(resolution)*2.0 * pi; + double x = mProperties.mRadius * cos(theta); + double y = mProperties.mRadius * sin(theta); + + // Back of head cone + mTriMesh->addVertex(Eigen::Vector3d(x, y, headBaseZ)); + // Head widened base + mTriMesh->addVertex(Eigen::Vector3d( + x * mProperties.mHeadRadiusScale, + y * mProperties.mHeadRadiusScale, + headBaseZ)); } + // Head tip vertex + mTriMesh->addVertex(Eigen::Vector3d(0, 0, headTipZ)); - // construct the head - constructArrowTip( - mMesh->mMeshes[2], length - headLength, length, mProperties); + // Head triangles + for (std::size_t i = 0; i < resolution; ++i) { + std::size_t next = (i + 1) % resolution; + + // Back face triangles + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i, + vertexOffset + 2 * next + 1, + vertexOffset + 2 * i + 1)); + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i, + vertexOffset + 2 * next, + vertexOffset + 2 * next + 1)); + + // Cone tip triangles + mTriMesh->addTriangle(math::TriMesh::Triangle( + vertexOffset + 2 * i + 1, + vertexOffset + 2 * next + 1, + vertexOffset + 2 * resolution)); + } + // Apply transformation to orient arrow from tail to head Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translation() = mTail; Eigen::Vector3d v = mHead - mTail; @@ -223,18 +317,25 @@ void ArrowShape::configureArrow( if (v.norm() > 0) { v.normalize(); Eigen::Vector3d axis = z.cross(v); - if (axis.norm() > 0) + if (axis.norm() > 0) { axis.normalize(); - else - axis - = Eigen::Vector3d::UnitY(); // Any vector in the X/Y plane can be used - tf.rotate(Eigen::AngleAxisd(acos(z.dot(v)), axis)); + tf.rotate(Eigen::AngleAxisd(acos(z.dot(v)), axis)); + } else { + // v is parallel or antiparallel to z + if (z.dot(v) < 0) { + // v is antiparallel to z, rotate 180 degrees around any perpendicular + // axis + tf.rotate(Eigen::AngleAxisd(pi, Eigen::Vector3d::UnitY())); + } + // If v is parallel to z, no rotation needed (identity) + } } - aiNode* node = mMesh->mRootNode; - for (std::size_t i = 0; i < 4; ++i) - for (std::size_t j = 0; j < 4; ++j) - node->mTransformation[i][j] = tf(i, j); + // Transform all vertices - need to get non-const access + auto& vertices = mTriMesh->getVertices(); + for (auto& vertex : vertices) { + vertex = tf * vertex; + } mIsBoundingBoxDirty = true; mIsVolumeDirty = true; @@ -245,21 +346,25 @@ void ArrowShape::configureArrow( //============================================================================== ShapePtr ArrowShape::clone() const { - aiScene* new_scene = cloneMesh(); auto new_shape = std::make_shared(); new_shape->mTail = mTail; new_shape->mHead = mHead; new_shape->mProperties = mProperties; + new_shape->mResolution = mResolution; + + // Clone the TriMesh + new_shape->mTriMesh = std::make_shared>(*mTriMesh); - new_shape->setMesh( - new_scene, MeshOwnership::Copied, mMeshUri, mResourceRetriever); + new_shape->mMeshUri = mMeshUri; + new_shape->mResourceRetriever = mResourceRetriever; new_shape->mMeshPath = mMeshPath; new_shape->mDisplayList = mDisplayList; new_shape->mScale = mScale; new_shape->mColorMode = mColorMode; new_shape->mAlphaMode = mAlphaMode; new_shape->mColorIndex = mColorIndex; + new_shape->mMaterials = mMaterials; return new_shape; } @@ -267,131 +372,7 @@ ShapePtr ArrowShape::clone() const //============================================================================== void ArrowShape::instantiate(std::size_t resolution) { - aiNode* node = new aiNode; - node->mNumMeshes = 3; - node->mMeshes = new unsigned int[3]; - for (std::size_t i = 0; i < 3; ++i) - node->mMeshes[i] = i; - - aiScene* scene = new aiScene; - scene->mNumMeshes = 3; - scene->mMeshes = new aiMesh*[3]; - scene->mRootNode = node; - - // Explicitly record the material count so Assimp's destructor cleans it up. - scene->mNumMaterials = 1; - scene->mMaterials = new aiMaterial*[1]; - scene->mMaterials[0] = new aiMaterial; - - // allocate memory - for (std::size_t i = 0; i < 3; ++i) { - std::size_t numVertices - = (i == 0 || i == 2) ? 2 * resolution + 1 : 2 * resolution; - - aiMesh* mesh = new aiMesh; - mesh->mMaterialIndex = (unsigned int)(-1); - - mesh->mNumVertices = numVertices; - mesh->mVertices = new aiVector3D[numVertices]; - mesh->mNormals = new aiVector3D[numVertices]; - mesh->mColors[0] = new aiColor4D[numVertices]; - - std::size_t numFaces = (i == 0 || i == 2) ? 3 * resolution : numVertices; - mesh->mNumFaces = numFaces; - mesh->mFaces = new aiFace[numFaces]; - for (std::size_t j = 0; j < numFaces; ++j) { - mesh->mFaces[j].mNumIndices = 3; - mesh->mFaces[j].mIndices = new unsigned int[3]; - } - - scene->mMeshes[i] = mesh; - } - - // set normals - aiMesh* mesh = scene->mMeshes[0]; - for (std::size_t i = 0; i < resolution; ++i) { - mesh->mNormals[2 * i].Set(0.0f, 0.0f, 1.0f); - - double theta = (double)(i) / (double)(resolution)*2 * math::pi; - mesh->mNormals[2 * i + 1].Set(cos(theta), sin(theta), 0.0f); - } - mesh->mNormals[mesh->mNumVertices - 1].Set(0.0f, 0.0f, -1.0f); - - mesh = scene->mMeshes[1]; - for (std::size_t i = 0; i < resolution; ++i) { - double theta = (double)(i) / (double)(resolution)*2 * math::pi; - mesh->mNormals[2 * i].Set(cos(theta), sin(theta), 0.0f); - mesh->mNormals[2 * i + 1].Set(cos(theta), sin(theta), 0.0f); - } - - mesh = scene->mMeshes[2]; - for (std::size_t i = 0; i < resolution; ++i) { - mesh->mNormals[2 * i].Set(0.0f, 0.0f, -1.0f); - - double theta = (double)(i) / (double)(resolution)*2 * math::pi; - mesh->mNormals[2 * i + 1].Set(cos(theta), sin(theta), 0.0f); - } - mesh->mNormals[mesh->mNumVertices - 1].Set(0.0f, 0.0f, 1.0f); - - // set faces - mesh = scene->mMeshes[0]; - aiFace* face; - for (std::size_t i = 0; i < resolution; ++i) { - // Back of head - face = &mesh->mFaces[3 * i]; - face->mIndices[0] = 2 * i; - face->mIndices[1] = 2 * i + 1; - face->mIndices[2] = (i + 1 < resolution) ? 2 * i + 3 : 1; - - face = &mesh->mFaces[3 * i + 1]; - face->mIndices[0] = 2 * i; - face->mIndices[1] = (i + 1 < resolution) ? 2 * i + 3 : 1; - face->mIndices[2] = (i + 1 < resolution) ? 2 * i + 2 : 0; - - // Tip - face = &mesh->mFaces[3 * i + 2]; - face->mIndices[0] = 2 * i + 1; - face->mIndices[1] = 2 * resolution; - face->mIndices[2] = (i + 1 < resolution) ? 2 * i + 3 : 1; - } - - mesh = scene->mMeshes[1]; - for (std::size_t i = 0; i < resolution; ++i) { - face = &mesh->mFaces[2 * i]; - face->mIndices[0] = 2 * i; - face->mIndices[1] = (i + 1 < resolution) ? 2 * i + 3 : 1; - face->mIndices[2] = 2 * i + 1; - - face = &mesh->mFaces[2 * i + 1]; - face->mIndices[0] = 2 * i; - face->mIndices[1] = (i + 1 < resolution) ? 2 * i + 2 : 0; - face->mIndices[2] = (i + 1 < resolution) ? 2 * i + 3 : 1; - } - - mesh = scene->mMeshes[2]; - for (std::size_t i = 0; i < resolution; ++i) { - // Back of head - face = &mesh->mFaces[3 * i]; - face->mIndices[0] = 2 * i; - face->mIndices[1] = (i + 1 < resolution) ? 2 * i + 3 : 1; - face->mIndices[2] = 2 * i + 1; - - face = &mesh->mFaces[3 * i + 1]; - face->mIndices[0] = 2 * i; - face->mIndices[1] = (i + 1 < resolution) ? 2 * i + 2 : 0; - face->mIndices[2] = (i + 1 < resolution) ? 2 * i + 3 : 1; - - // Tip - face = &mesh->mFaces[3 * i + 2]; - face->mIndices[0] = 2 * i + 1; - face->mIndices[1] = (i + 1 < resolution) ? 2 * i + 3 : 1; - face->mIndices[2] = 2 * resolution; - } - - setMesh(scene, MeshOwnership::Manual, common::Uri(), nullptr); - - // setColor(mColor); - // TODO(JS) + mResolution = resolution; } } // namespace dynamics diff --git a/dart/dynamics/ArrowShape.hpp b/dart/dynamics/ArrowShape.hpp index 2d36b4bf4a24c..40158d730aa2d 100644 --- a/dart/dynamics/ArrowShape.hpp +++ b/dart/dynamics/ArrowShape.hpp @@ -79,6 +79,9 @@ class DART_API ArrowShape : public MeshShape const Eigen::Vector4d& _color = Eigen::Vector4d(0.5, 0.5, 1.0, 1.0), std::size_t _resolution = 10); + /// Destructor + ~ArrowShape() override; + /// Set the positions of the tail and head of the arrow without changing any /// settings void setPositions(const Eigen::Vector3d& _tail, const Eigen::Vector3d& _head); @@ -113,6 +116,9 @@ class DART_API ArrowShape : public MeshShape Eigen::Vector3d mHead; Properties mProperties; + + /// Resolution used for mesh generation + std::size_t mResolution; }; } // namespace dynamics diff --git a/dart/dynamics/AssimpInputResourceAdaptor.hpp b/dart/dynamics/AssimpInputResourceAdaptor.hpp index fc4e3a9a18c28..92bea1394cbd7 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.hpp +++ b/dart/dynamics/AssimpInputResourceAdaptor.hpp @@ -30,102 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_HPP_ -#define DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_HPP_ +#ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_PUBLIC_HPP_ +#define DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_PUBLIC_HPP_ -#include -#include +// Backward-compatibility header. +#include -#include - -#include -#include -#include - -namespace dart { -namespace dynamics { - -class DART_API AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem -{ -public: - explicit AssimpInputResourceRetrieverAdaptor( - const common::ResourceRetrieverPtr& _resourceRetriever); - virtual ~AssimpInputResourceRetrieverAdaptor(); - - /// @brief Tests for the existence of a file at the given path. - /// - /// @param pFile Path to the file - /// @return true if there is a file with this path, else false. - bool Exists(const char* pFile) const override; - - /// @brief Returns the system specific directory separator - /// - /// @return System specific directory separator - char getOsSeparator() const override; - - /// @brief Open a new file with a given path. - /// - /// When the access to the file is finished, call Close() to release - /// all associated resources (or the virtual dtor of the IOStream). - /// - /// @param pFile Path to the file - /// @param pMode Desired file I/O mode. Required are: "wb", "w", "wt", - /// "rb", "r", "rt". - /// @return New IOStream interface allowing the lib to access - /// the underlying file. - Assimp::IOStream* Open(const char* pFile, const char* pMode = "rb") override; - - /// @brief Closes the given file and releases all resources - /// associated with it. - /// @param pFile The file instance previously created by Open(). - void Close(Assimp::IOStream* pFile) override; - -private: - common::ResourceRetrieverPtr mResourceRetriever; -}; - -class DART_API AssimpInputResourceAdaptor : public Assimp::IOStream -{ -public: - explicit AssimpInputResourceAdaptor(const common::ResourcePtr& _resource); - virtual ~AssimpInputResourceAdaptor(); - - /// @brief Read from the file - /// - /// See fread() for more details - std::size_t Read( - void* pvBuffer, std::size_t pSize, std::size_t pCount) override; - - /// @brief Not implemented. This is a read-only stream. - std::size_t Write( - const void* pvBuffer, std::size_t pSize, std::size_t pCount) override; - - /// @brief Set the read/write cursor of the file - /// - /// Note that the offset is _negative_ for aiOrigin_END. - /// See fseek() for more details - aiReturn Seek(std::size_t pOffset, aiOrigin pOrigin) override; - - /// @brief Get the current position of the read/write cursor - /// - /// See ftell() for more details - std::size_t Tell() const override; - - /// @brief Returns filesize - /// - /// Returns the filesize. - std::size_t FileSize() const override; - - /// @brief Not implemented. This is a read-only stream. - void Flush() override; - -private: - common::ResourcePtr mResource; -}; - -DART_API aiFileIO createFileIO(Assimp::IOSystem* adaptor); - -} // namespace dynamics -} // namespace dart - -#endif // ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_HPP_ +#endif // DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_PUBLIC_HPP_ diff --git a/dart/dynamics/MeshMaterial.hpp b/dart/dynamics/MeshMaterial.hpp new file mode 100644 index 0000000000000..0530e9289e6a7 --- /dev/null +++ b/dart/dynamics/MeshMaterial.hpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_MESHMATERIAL_HPP_ +#define DART_DYNAMICS_MESHMATERIAL_HPP_ + +#include + +#include +#include + +namespace dart { +namespace dynamics { + +/// Simple material representation for mesh rendering +/// Stores material properties independently of Assimp types +struct MeshMaterial +{ + /// Material colors + Eigen::Vector4f ambient{0.2f, 0.2f, 0.2f, 1.0f}; + Eigen::Vector4f diffuse{0.8f, 0.8f, 0.8f, 1.0f}; + Eigen::Vector4f specular{0.0f, 0.0f, 0.0f, 1.0f}; + Eigen::Vector4f emissive{0.0f, 0.0f, 0.0f, 1.0f}; + + /// Shininess coefficient + float shininess{0.0f}; + + /// Texture image paths or URIs (absolute when resolved on the filesystem) + /// Index 0: diffuse texture + /// Index 1: specular texture + /// Index 2: normal texture + /// etc. + std::vector textureImagePaths; + + /// Default constructor + MeshMaterial() = default; +}; + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_MESHMATERIAL_HPP_ diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 55d9fd32083bd..08c19adba7ccc 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -33,13 +33,15 @@ #include "dart/dynamics/MeshShape.hpp" #include "dart/common/Diagnostics.hpp" +#include "dart/common/Filesystem.hpp" #include "dart/common/LocalResourceRetriever.hpp" #include "dart/common/Logging.hpp" #include "dart/common/Resource.hpp" #include "dart/common/Uri.hpp" #include "dart/config.hpp" -#include "dart/dynamics/AssimpInputResourceAdaptor.hpp" #include "dart/dynamics/BoxShape.hpp" +#include "dart/dynamics/MeshMaterial.hpp" +#include "dart/dynamics/detail/AssimpInputResourceAdaptor.hpp" #include #include @@ -48,28 +50,316 @@ #include #include +#include #include +#include +#include +#include #include +#include +#include + +#include namespace dart { namespace dynamics { +namespace { + +class MemoryResource final : public common::Resource +{ +public: + explicit MemoryResource(std::string data) + : mData(std::move(data)), mPosition(0) + { + } + + std::size_t getSize() override + { + return mData.size(); + } + + std::size_t tell() override + { + return mPosition; + } + + bool seek(ptrdiff_t offset, SeekType origin) override + { + ptrdiff_t base = 0; + switch (origin) { + case SEEKTYPE_CUR: + base = static_cast(mPosition); + break; + case SEEKTYPE_END: + base = static_cast(mData.size()); + break; + case SEEKTYPE_SET: + base = 0; + break; + default: + return false; + } + + const ptrdiff_t next = base + offset; + if (next < 0) + return false; + if (static_cast(next) > mData.size()) + return false; + + mPosition = static_cast(next); + return true; + } + + std::size_t read(void* buffer, std::size_t size, std::size_t count) override + { + if (!buffer || size == 0 || count == 0) + return 0; + + const std::size_t remaining = mData.size() - mPosition; + const std::size_t availableCount = remaining / size; + const std::size_t toReadCount = std::min(count, availableCount); + const std::size_t toReadBytes = toReadCount * size; + + if (toReadBytes > 0) { + std::memcpy(buffer, mData.data() + mPosition, toReadBytes); + mPosition += toReadBytes; + } + + return toReadCount; + } + +private: + std::string mData; + std::size_t mPosition; +}; + +class MemoryResourceRetriever final : public common::ResourceRetriever +{ +public: + MemoryResourceRetriever( + std::unordered_map data, + common::ResourceRetrieverPtr fallback) + : mData(std::move(data)), mFallback(std::move(fallback)) + { + } + + bool exists(const common::Uri& uri) override + { + return findData(uri) != nullptr || (mFallback && mFallback->exists(uri)); + } + + common::ResourcePtr retrieve(const common::Uri& uri) override + { + const std::string* data = findData(uri); + if (data) + return std::make_shared(*data); + + if (mFallback) + return mFallback->retrieve(uri); + + return nullptr; + } + +private: + const std::string* findData(const common::Uri& uri) const + { + const auto byFull = mData.find(uri.toString()); + if (byFull != mData.end()) + return &byFull->second; + + const auto findByPath = [&](const std::string& path) -> const std::string* { + const auto byPath = mData.find(path); + if (byPath != mData.end()) + return &byPath->second; + + if (!path.empty() && path.front() == '/') { + const auto byTrim = mData.find(path.substr(1)); + if (byTrim != mData.end()) + return &byTrim->second; + } + + const auto slash = path.find_last_of("/\\"); + if (slash != std::string::npos) { + const auto byName = mData.find(path.substr(slash + 1)); + if (byName != mData.end()) + return &byName->second; + } + + return nullptr; + }; + + if (uri.mPath) { + if (const std::string* data = findByPath(uri.mPath.get())) + return data; + } + + if (uri.mAuthority) { + if (const std::string* data = findByPath(uri.mAuthority.get())) + return data; + } + + return nullptr; + } + + std::unordered_map mData; + common::ResourceRetrieverPtr mFallback; +}; + +} // namespace + +//============================================================================== +bool MeshShape::collectSubMeshRanges( + const aiScene* scene, + std::vector& ranges, + std::size_t expectedVertices, + std::size_t expectedTriangles) +{ + ranges.clear(); + if (!scene) + return false; + + std::size_t vertexOffset = 0; + std::size_t triangleOffset = 0; + + for (std::size_t meshIndex = 0; meshIndex < scene->mNumMeshes; ++meshIndex) { + const aiMesh* assimpMesh = scene->mMeshes[meshIndex]; + if (!assimpMesh) { + continue; + } + + std::size_t triangleCount = 0; + for (auto i = 0u; i < assimpMesh->mNumFaces; ++i) { + const aiFace& face = assimpMesh->mFaces[i]; + if (face.mNumIndices == 3) + ++triangleCount; + } + + SubMeshRange range; + range.vertexOffset = vertexOffset; + range.vertexCount = assimpMesh->mNumVertices; + range.triangleOffset = triangleOffset; + range.triangleCount = triangleCount; + range.materialIndex = assimpMesh->mMaterialIndex; + ranges.push_back(range); + + vertexOffset += assimpMesh->mNumVertices; + triangleOffset += triangleCount; + } + + if (expectedVertices != 0 && vertexOffset != expectedVertices) { + ranges.clear(); + return false; + } + if (expectedTriangles != 0 && triangleOffset != expectedTriangles) { + ranges.clear(); + return false; + } + + return !ranges.empty(); +} + //============================================================================== MeshShape::MeshShape( const Eigen::Vector3d& scale, const aiScene* mesh, - const common::Uri& path, + const common::Uri& uri, common::ResourceRetrieverPtr resourceRetriever, MeshOwnership ownership) : Shape(MESH), - mMesh(nullptr), - mMeshOwnership(MeshOwnership::None), + mTriMesh(nullptr), + mCachedAiScene(nullptr), + mDisplayList(0), + mScale(scale), + mColorMode(MATERIAL_COLOR), + mAlphaMode(BLEND), + mColorIndex(0) +{ + DART_SUPPRESS_DEPRECATED_BEGIN + setMesh(mesh, ownership, uri, std::move(resourceRetriever)); + DART_SUPPRESS_DEPRECATED_END + setScale(scale); +} + +//============================================================================== +MeshShape::MeshShape( + const Eigen::Vector3d& scale, + std::shared_ptr> mesh, + const common::Uri& uri) + : MeshShape(scale, std::move(mesh), uri, nullptr) +{ +} + +//============================================================================== +MeshShape::MeshShape( + const Eigen::Vector3d& scale, + std::shared_ptr> mesh, + const common::Uri& uri, + common::ResourceRetrieverPtr resourceRetriever) + : Shape(MESH), + mTriMesh(std::move(mesh)), + mCachedAiScene(nullptr), + mMeshUri(uri), + mMeshPath(), + mResourceRetriever(std::move(resourceRetriever)), mDisplayList(0), + mScale(scale), mColorMode(MATERIAL_COLOR), mAlphaMode(BLEND), mColorIndex(0) { - setMesh(mesh, ownership, path, std::move(resourceRetriever)); + if (uri.mScheme.get_value_or("file") == "file" && uri.mPath) { + mMeshPath = uri.getFilesystemPath(); + } else if (mResourceRetriever) { + DART_SUPPRESS_DEPRECATED_BEGIN + mMeshPath = mResourceRetriever->getFilePath(uri); + DART_SUPPRESS_DEPRECATED_END + } else { + mMeshPath.clear(); + } + + mMaterials.clear(); + + const std::string uriString = uri.toString(); + if (!uriString.empty()) { + common::ResourceRetrieverPtr materialRetriever = mResourceRetriever; + if (!materialRetriever && uri.mScheme.get_value_or("file") == "file" + && uri.mPath) { + materialRetriever = std::make_shared(); + } + + if (materialRetriever) { + struct AiSceneReleaser + { + void operator()(const aiScene* scene) const + { + if (scene) + aiReleaseImport(const_cast(scene)); + } + }; + + DART_SUPPRESS_DEPRECATED_BEGIN + const std::unique_ptr scene( + loadMesh(uri, materialRetriever)); + DART_SUPPRESS_DEPRECATED_END + + if (scene) { + extractMaterialsFromScene( + scene.get(), + mMeshPath.empty() ? uri.getPath() : mMeshPath, + mMeshUri); + + const auto expectedVertices + = mTriMesh ? mTriMesh->getVertices().size() : 0; + const auto expectedTriangles + = mTriMesh ? mTriMesh->getTriangles().size() : 0; + collectSubMeshRanges( + scene.get(), mSubMeshRanges, expectedVertices, expectedTriangles); + extractTextureCoordsFromScene(scene.get()); + } + } + } + setScale(scale); } @@ -77,33 +367,138 @@ MeshShape::MeshShape( MeshShape::MeshShape( const Eigen::Vector3d& scale, std::shared_ptr mesh, - const common::Uri& path, + const common::Uri& uri, common::ResourceRetrieverPtr resourceRetriever) : Shape(MESH), - mMesh(nullptr), - mMeshOwnership(MeshOwnership::None), + mTriMesh(nullptr), + mCachedAiScene(nullptr), mDisplayList(0), + mScale(scale), mColorMode(MATERIAL_COLOR), mAlphaMode(BLEND), mColorIndex(0) { - setMesh(std::move(mesh), path, std::move(resourceRetriever)); + DART_SUPPRESS_DEPRECATED_BEGIN + setMesh(std::move(mesh), uri, std::move(resourceRetriever)); + DART_SUPPRESS_DEPRECATED_END setScale(scale); } //============================================================================== MeshShape::~MeshShape() { + // Clean up cached aiScene if it was created. + if (mCachedAiScene) { + aiReleaseImport(mCachedAiScene); + mCachedAiScene = nullptr; + } + releaseMesh(); } //============================================================================== void MeshShape::releaseMesh() +{ + mMesh.reset(); +} + +//============================================================================== +std::shared_ptr MeshShape::makeMeshHandle( + const aiScene* mesh, MeshOwnership ownership) +{ + if (!mesh) + return nullptr; + + switch (ownership) { + case MeshOwnership::Imported: + return std::shared_ptr(mesh, [](const aiScene* scene) { + aiReleaseImport(const_cast(scene)); + }); + case MeshOwnership::Copied: + return std::shared_ptr(mesh, [](const aiScene* scene) { + aiFreeScene(const_cast(scene)); + }); + case MeshOwnership::Manual: + return std::shared_ptr(mesh, [](const aiScene* scene) { + delete const_cast(scene); + }); + case MeshOwnership::Custom: + case MeshOwnership::None: + default: + return std::shared_ptr(mesh, [](const aiScene*) {}); + } +} + +//============================================================================== +MeshShape::MeshHandle& MeshShape::MeshHandle::operator=(const aiScene* mesh) +{ + // Backward compatibility: historically MeshShape exposed a std::shared_ptr + // member that derived classes could assign a manually constructed aiScene* + // into (e.g., Gazebo's CustomMeshShape). In that case the default deleter + // would invoke `delete`, so we preserve that behavior here. + set(mesh, MeshOwnership::Manual); + return *this; +} + +//============================================================================== +MeshShape::MeshHandle& MeshShape::MeshHandle::operator=( + std::shared_ptr mesh) +{ + set(std::move(mesh)); + return *this; +} + +//============================================================================== +const aiScene* MeshShape::MeshHandle::get() const +{ + return mMesh.get(); +} + +//============================================================================== +const aiScene* MeshShape::MeshHandle::operator->() const +{ + return mMesh.get(); +} + +//============================================================================== +MeshShape::MeshHandle::operator bool() const +{ + return static_cast(mMesh); +} + +//============================================================================== +void MeshShape::MeshHandle::reset() { mMesh.reset(); mMeshOwnership = MeshOwnership::None; } +//============================================================================== +MeshShape::MeshOwnership MeshShape::MeshHandle::getOwnership() const +{ + return mMeshOwnership; +} + +//============================================================================== +const std::shared_ptr& MeshShape::MeshHandle::getShared() const +{ + return mMesh; +} + +//============================================================================== +void MeshShape::MeshHandle::set(const aiScene* mesh, MeshOwnership ownership) +{ + mMesh = MeshShape::makeMeshHandle(mesh, ownership); + mMeshOwnership = mesh ? ownership : MeshOwnership::None; +} + +//============================================================================== +void MeshShape::MeshHandle::set(std::shared_ptr mesh) +{ + mMesh = std::move(mesh); + mMeshOwnership = mMesh ? MeshOwnership::Custom : MeshOwnership::None; +} + //============================================================================== const std::string& MeshShape::getType() const { @@ -117,10 +512,377 @@ const std::string& MeshShape::getStaticType() return type; } +//============================================================================== +std::shared_ptr> MeshShape::getTriMesh() const +{ + // Handle backward compatibility: if a derived class bypasses setMesh() and + // updates the underlying aiScene directly, we lazily populate the TriMesh. + if (!mTriMesh) { + const aiScene* scene = nullptr; + if (mMesh) { + scene = mMesh.get(); + } else if (mCachedAiScene) { + scene = mCachedAiScene; + } + + if (scene) { + // Const cast is safe here - we're lazily populating internal cache. + auto* self = const_cast(this); + self->mTriMesh = convertAssimpMesh(scene, &self->mSubMeshRanges); + self->extractTextureCoordsFromScene(scene); + } + } + return mTriMesh; +} + //============================================================================== const aiScene* MeshShape::getMesh() const { - return mMesh.get(); + if (mMesh) { + return mMesh.get(); + } + + // Lazy conversion: only convert once and cache the result. + // NOTE: This is still expensive on first call! Please use getTriMesh() + // instead. + if (!mCachedAiScene) { + mCachedAiScene = convertToAssimpMesh(); + } + + return mCachedAiScene; +} + +//============================================================================== +const aiScene* MeshShape::convertToAssimpMesh() const +{ + if (!mTriMesh || !mTriMesh->hasTriangles()) { + return nullptr; + } + + // Assimp's aiScene is not designed as a mutable in-memory structure and its + // lifetime rules are complex (see #453). To keep getMesh() working for legacy + // callers while avoiding cross-module deallocation hazards, we round-trip the + // TriMesh through Assimp's importer so Assimp allocates and frees the scene. + + std::ostringstream stream; + stream.imbue(std::locale::classic()); + stream << std::setprecision(std::numeric_limits::max_digits10); + + const auto& vertices = mTriMesh->getVertices(); + for (const auto& vertex : vertices) { + stream << "v " << vertex.x() << ' ' << vertex.y() << ' ' << vertex.z() + << '\n'; + } + + const bool hasTexCoords = !mTextureCoords.empty() + && mTextureCoords.size() == vertices.size() + && mTextureCoordComponents > 0; + if (hasTexCoords) { + const int components = std::min(mTextureCoordComponents, 3); + for (const auto& texCoord : mTextureCoords) { + stream << "vt " << texCoord.x(); + if (components >= 2) { + stream << ' ' << texCoord.y(); + } + if (components >= 3) { + stream << ' ' << texCoord.z(); + } + stream << '\n'; + } + } + + const auto& normals = mTriMesh->getVertexNormals(); + const bool hasNormals = !normals.empty() && normals.size() == vertices.size(); + if (hasNormals) { + for (const auto& normal : normals) { + stream << "vn " << normal.x() << ' ' << normal.y() << ' ' << normal.z() + << '\n'; + } + } + + const auto& triangles = mTriMesh->getTriangles(); + const bool hasSubMeshes = !mSubMeshRanges.empty() && mMaterials.size() > 1; + bool useSubMeshes = hasSubMeshes; + if (useSubMeshes) { + std::size_t totalVertices = 0; + std::size_t totalTriangles = 0; + for (const auto& range : mSubMeshRanges) { + if (range.vertexOffset + range.vertexCount > vertices.size() + || range.triangleOffset + range.triangleCount > triangles.size()) { + useSubMeshes = false; + break; + } + totalVertices += range.vertexCount; + totalTriangles += range.triangleCount; + } + if (useSubMeshes + && (totalVertices != vertices.size() + || totalTriangles != triangles.size())) { + useSubMeshes = false; + } + } + + std::ostringstream mtlStream; + std::string mtlName; + if (useSubMeshes) { + mtlName = "dart_mesh.mtl"; + stream << "mtllib " << mtlName << '\n'; + } + + if (useSubMeshes) { + bool warnedIndex = false; + for (const auto& range : mSubMeshRanges) { + if (range.triangleCount == 0) + continue; + + unsigned int materialIndex = range.materialIndex; + if (materialIndex >= mMaterials.size()) { + if (!warnedIndex) { + DART_WARN( + "[MeshShape::convertToAssimpMesh] Material index {} is out of " + "range (materials: {}). Falling back to material 0.", + materialIndex, + mMaterials.size()); + warnedIndex = true; + } + materialIndex = 0; + } + + stream << "usemtl material_" << materialIndex << '\n'; + for (std::size_t i = 0; i < range.triangleCount; ++i) { + const auto& triangle = triangles[range.triangleOffset + i]; + const std::size_t i0 = static_cast(triangle.x()) + 1; + const std::size_t i1 = static_cast(triangle.y()) + 1; + const std::size_t i2 = static_cast(triangle.z()) + 1; + + if (hasTexCoords && hasNormals) { + stream << "f " << i0 << "/" << i0 << "/" << i0 << ' ' << i1 << "/" + << i1 << "/" << i1 << ' ' << i2 << "/" << i2 << "/" << i2 + << '\n'; + } else if (hasTexCoords) { + stream << "f " << i0 << "/" << i0 << ' ' << i1 << "/" << i1 << ' ' + << i2 << "/" << i2 << '\n'; + } else if (hasNormals) { + stream << "f " << i0 << "//" << i0 << ' ' << i1 << "//" << i1 << ' ' + << i2 << "//" << i2 << '\n'; + } else { + stream << "f " << i0 << ' ' << i1 << ' ' << i2 << '\n'; + } + } + } + + mtlStream.imbue(std::locale::classic()); + mtlStream << std::setprecision(std::numeric_limits::max_digits10); + for (std::size_t index = 0; index < mMaterials.size(); ++index) { + const auto& mat = mMaterials[index]; + mtlStream << "newmtl material_" << index << '\n'; + mtlStream << "Ka " << mat.ambient[0] << ' ' << mat.ambient[1] << ' ' + << mat.ambient[2] << '\n'; + mtlStream << "Kd " << mat.diffuse[0] << ' ' << mat.diffuse[1] << ' ' + << mat.diffuse[2] << '\n'; + mtlStream << "Ks " << mat.specular[0] << ' ' << mat.specular[1] << ' ' + << mat.specular[2] << '\n'; + mtlStream << "Ke " << mat.emissive[0] << ' ' << mat.emissive[1] << ' ' + << mat.emissive[2] << '\n'; + mtlStream << "Ns " << mat.shininess << '\n'; + mtlStream << "d " << mat.diffuse[3] << '\n'; + + if (!mat.textureImagePaths.empty() && !mat.textureImagePaths[0].empty()) { + mtlStream << "map_Kd " << mat.textureImagePaths[0] << '\n'; + } + mtlStream << '\n'; + } + } else { + for (const auto& triangle : triangles) { + const std::size_t i0 = static_cast(triangle.x()) + 1; + const std::size_t i1 = static_cast(triangle.y()) + 1; + const std::size_t i2 = static_cast(triangle.z()) + 1; + + if (hasTexCoords && hasNormals) { + stream << "f " << i0 << "/" << i0 << "/" << i0 << ' ' << i1 << "/" << i1 + << "/" << i1 << ' ' << i2 << "/" << i2 << "/" << i2 << '\n'; + } else if (hasTexCoords) { + stream << "f " << i0 << "/" << i0 << ' ' << i1 << "/" << i1 << ' ' << i2 + << "/" << i2 << '\n'; + } else if (hasNormals) { + stream << "f " << i0 << "//" << i0 << ' ' << i1 << "//" << i1 << ' ' + << i2 << "//" << i2 << '\n'; + } else { + stream << "f " << i0 << ' ' << i1 << ' ' << i2 << '\n'; + } + } + } + + const std::string obj = stream.str(); + if (obj.empty()) { + return nullptr; + } + + const unsigned int flags = aiProcess_Triangulate + | aiProcess_JoinIdenticalVertices + | aiProcess_SortByPType | aiProcess_OptimizeMeshes + | aiProcess_GenNormals; + + if (!useSubMeshes) { + const aiScene* scene = aiImportFileFromMemory( + obj.data(), static_cast(obj.size()), flags, "obj"); + + if (!scene) { + DART_WARN( + "[MeshShape::convertToAssimpMesh] Failed to import TriMesh via " + "Assimp: {}", + aiGetErrorString()); + return nullptr; + } + + return scene; + } + + const std::string objName = "dart_mesh.obj"; + std::unordered_map data; + data.emplace(objName, obj); + data.emplace(mtlName, mtlStream.str()); + + auto retriever = std::make_shared( + std::move(data), mResourceRetriever); + + aiPropertyStore* propertyStore = aiCreatePropertyStore(); + aiSetImportPropertyInteger( + propertyStore, + AI_CONFIG_PP_SBP_REMOVE, + aiPrimitiveType_POINT | aiPrimitiveType_LINE); + + AssimpInputResourceRetrieverAdaptor systemIO(retriever); + aiFileIO fileIO = createFileIO(&systemIO); + + const aiScene* scene = aiImportFileExWithProperties( + objName.c_str(), flags, &fileIO, propertyStore); + + aiReleasePropertyStore(propertyStore); + + if (!scene) { + DART_WARN( + "[MeshShape::convertToAssimpMesh] Failed to import TriMesh with " + "materials via Assimp: {}", + aiGetErrorString()); + return nullptr; + } + + return scene; +} + +//============================================================================== +std::shared_ptr> MeshShape::convertAssimpMesh( + const aiScene* scene) +{ + return convertAssimpMesh(scene, nullptr); +} + +//============================================================================== +std::shared_ptr> MeshShape::convertAssimpMesh( + const aiScene* scene, std::vector* subMeshes) +{ + if (!scene) { + return nullptr; + } + + auto triMesh = std::make_shared>(); + if (subMeshes) { + subMeshes->clear(); + } + + // ALWAYS merge all meshes into a single TriMesh + // This is necessary because: + // 1. Files with multiple materials (common in COLLADA/OBJ) have multiple + // aiMesh objects + // 2. All meshes in a scene belong to the same shape for collision/rendering + // 3. CustomMeshShape from gz-physics creates one aiMesh per submesh + const std::size_t numMeshesToProcess = scene->mNumMeshes; + + // Reserve space for all vertices and faces upfront + std::size_t totalVertices = 0; + std::size_t totalFaces = 0; + for (std::size_t i = 0; i < numMeshesToProcess; ++i) { + if (scene->mMeshes[i]) { + totalVertices += scene->mMeshes[i]->mNumVertices; + totalFaces += scene->mMeshes[i]->mNumFaces; + } + } + triMesh->reserveVertices(totalVertices); + triMesh->reserveTriangles(totalFaces); + triMesh->reserveVertexNormals(totalVertices); + + // Process all meshes and merge them into a single TriMesh + for (std::size_t meshIndex = 0; meshIndex < numMeshesToProcess; ++meshIndex) { + const aiMesh* assimpMesh = scene->mMeshes[meshIndex]; + + if (!assimpMesh) { + continue; + } + + // Track the vertex offset for this submesh (for face indices) + const std::size_t vertexOffset = triMesh->getVertices().size(); + const std::size_t triangleOffset = triMesh->getTriangles().size(); + std::size_t triangleCount = 0; + + // Parse vertices + for (auto i = 0u; i < assimpMesh->mNumVertices; ++i) { + const aiVector3D& vertex = assimpMesh->mVertices[i]; + triMesh->addVertex( + static_cast(vertex.x), + static_cast(vertex.y), + static_cast(vertex.z)); + } + + // Parse faces (triangles), adjusting indices by vertex offset + for (auto i = 0u; i < assimpMesh->mNumFaces; ++i) { + const aiFace& face = assimpMesh->mFaces[i]; + + // Skip non-triangular faces + if (face.mNumIndices != 3) { + DART_WARN( + "[MeshShape::convertAssimpMesh] Non-triangular face detected in " + "mesh {}. Skipping this face.", + meshIndex); + continue; + } + + triMesh->addTriangle( + face.mIndices[0] + vertexOffset, + face.mIndices[1] + vertexOffset, + face.mIndices[2] + vertexOffset); + ++triangleCount; + } + + // Parse vertex normals + if (assimpMesh->mNormals) { + for (auto i = 0u; i < assimpMesh->mNumVertices; ++i) { + const aiVector3D& normal = assimpMesh->mNormals[i]; + triMesh->addVertexNormal( + static_cast(normal.x), + static_cast(normal.y), + static_cast(normal.z)); + } + } + + if (subMeshes) { + SubMeshRange range; + range.vertexOffset = vertexOffset; + range.vertexCount = assimpMesh->mNumVertices; + range.triangleOffset = triangleOffset; + range.triangleCount = triangleCount; + range.materialIndex = assimpMesh->mMaterialIndex; + subMeshes->push_back(range); + } + } + + // Compute vertex normals if they are missing or incomplete. + if (triMesh->hasTriangles() + && triMesh->getVertexNormals().size() != triMesh->getVertices().size()) { + triMesh->computeVertexNormals(); + } + + return triMesh; } //============================================================================== @@ -159,11 +921,13 @@ void MeshShape::setMesh( const std::string& path, common::ResourceRetrieverPtr resourceRetriever) { + DART_SUPPRESS_DEPRECATED_BEGIN setMesh( mesh, MeshOwnership::Imported, common::Uri(path), std::move(resourceRetriever)); + DART_SUPPRESS_DEPRECATED_END } //============================================================================== @@ -172,41 +936,11 @@ void MeshShape::setMesh( const common::Uri& uri, common::ResourceRetrieverPtr resourceRetriever) { + DART_SUPPRESS_DEPRECATED_BEGIN setMesh(mesh, MeshOwnership::Imported, uri, std::move(resourceRetriever)); + DART_SUPPRESS_DEPRECATED_END } -//============================================================================== -namespace { - -std::shared_ptr makeMeshHandle( - const aiScene* mesh, MeshShape::MeshOwnership ownership) -{ - if (!mesh) - return nullptr; - - switch (ownership) { - case MeshShape::MeshOwnership::Imported: - return std::shared_ptr(mesh, [](const aiScene* scene) { // - aiReleaseImport(const_cast(scene)); - }); - case MeshShape::MeshOwnership::Copied: - return std::shared_ptr(mesh, [](const aiScene* scene) { - aiFreeScene(const_cast(scene)); - }); - case MeshShape::MeshOwnership::Manual: - return std::shared_ptr(mesh, [](const aiScene* scene) { - delete const_cast(scene); - }); - case MeshShape::MeshOwnership::Custom: - case MeshShape::MeshOwnership::None: - default: - return std::shared_ptr( - mesh, [](const aiScene*) { /* no-op */ }); - } -} - -} // namespace - //============================================================================== void MeshShape::setMesh( const aiScene* mesh, @@ -214,23 +948,54 @@ void MeshShape::setMesh( const common::Uri& uri, common::ResourceRetrieverPtr resourceRetriever) { - if (mesh == mMesh.get() && ownership == mMeshOwnership) { + if (mesh == mMesh.get() && ownership == mMesh.getOwnership()) { // Nothing to do. return; } + const bool meshIsCached = mesh && (mesh == mCachedAiScene); + const MeshOwnership effectiveOwnership + = meshIsCached ? MeshOwnership::Imported : ownership; + + if (meshIsCached && ownership != MeshOwnership::Imported) { + DART_WARN( + "[MeshShape::setMesh] Overriding MeshOwnership ({}) to Imported when " + "adopting a cached aiScene created by getMesh().", + static_cast(ownership)); + } + + // Clear cached aiScene to prevent stale data. If the caller is adopting the + // cached pointer, transfer ownership to mMesh without releasing it here. + if (mCachedAiScene) { + if (meshIsCached) { + mCachedAiScene = nullptr; + } else { + aiReleaseImport(mCachedAiScene); + mCachedAiScene = nullptr; + } + } + releaseMesh(); - mMesh = makeMeshHandle(mesh, ownership); - mMeshOwnership = mesh ? ownership : MeshOwnership::None; + mMesh.set(mesh, effectiveOwnership); + + mTriMesh = convertAssimpMesh(mMesh.get(), &mSubMeshRanges); + mMaterials.clear(); - if (!mMesh) { + if (!mTriMesh) { mMeshUri.clear(); mMeshPath.clear(); mResourceRetriever = nullptr; + mTextureCoords.clear(); + mTextureCoordComponents = 0; + mSubMeshRanges.clear(); + mIsBoundingBoxDirty = true; + mIsVolumeDirty = true; return; } + extractTextureCoordsFromScene(mMesh.get()); + mMeshUri = uri; if (uri.mScheme.get_value_or("file") == "file" && uri.mPath) { @@ -245,6 +1010,13 @@ void MeshShape::setMesh( mResourceRetriever = std::move(resourceRetriever); + // Extract material properties for Assimp-free rendering. + extractMaterialsFromScene( + mMesh.get(), mMeshPath.empty() ? uri.getPath() : mMeshPath, mMeshUri); + + mIsBoundingBoxDirty = true; + mIsVolumeDirty = true; + incrementVersion(); } @@ -254,21 +1026,40 @@ void MeshShape::setMesh( const common::Uri& uri, common::ResourceRetrieverPtr resourceRetriever) { - if (mesh == mMesh && mMeshOwnership == MeshOwnership::Custom) { + if (mesh && mesh.get() == mMesh.get() + && mMesh.getOwnership() == MeshOwnership::Custom) { return; } + if (mCachedAiScene) { + if (mesh && mesh.get() == mCachedAiScene) { + mCachedAiScene = nullptr; + } else { + aiReleaseImport(mCachedAiScene); + mCachedAiScene = nullptr; + } + } + releaseMesh(); - mMesh = std::move(mesh); - mMeshOwnership = mMesh ? MeshOwnership::Custom : MeshOwnership::None; + mMesh.set(std::move(mesh)); - if (!mMesh) { + mTriMesh = convertAssimpMesh(mMesh.get(), &mSubMeshRanges); + mMaterials.clear(); + + if (!mTriMesh) { mMeshUri.clear(); mMeshPath.clear(); mResourceRetriever = nullptr; + mTextureCoords.clear(); + mTextureCoordComponents = 0; + mSubMeshRanges.clear(); + mIsBoundingBoxDirty = true; + mIsVolumeDirty = true; return; } + extractTextureCoordsFromScene(mMesh.get()); + mMeshUri = uri; if (uri.mScheme.get_value_or("file") == "file" && uri.mPath) { @@ -283,6 +1074,13 @@ void MeshShape::setMesh( mResourceRetriever = std::move(resourceRetriever); + // Extract material properties for Assimp-free rendering. + extractMaterialsFromScene( + mMesh.get(), mMeshPath.empty() ? uri.getPath() : mMeshPath, mMeshUri); + + mIsBoundingBoxDirty = true; + mIsVolumeDirty = true; + incrementVersion(); } @@ -356,6 +1154,211 @@ void MeshShape::setDisplayList(int index) mDisplayList = index; } +//============================================================================== +const std::vector& MeshShape::getMaterials() const +{ + return mMaterials; +} + +//============================================================================== +std::size_t MeshShape::getNumMaterials() const +{ + return mMaterials.size(); +} + +//============================================================================== +const MeshMaterial* MeshShape::getMaterial(std::size_t index) const +{ + if (index < mMaterials.size()) { + return &mMaterials[index]; + } + return nullptr; +} + +//============================================================================== +void MeshShape::extractTextureCoordsFromScene(const aiScene* scene) +{ + mTextureCoords.clear(); + mTextureCoordComponents = 0; + + if (!scene || scene->mNumMeshes == 0) { + return; + } + + std::size_t totalVertices = 0; + bool hasTexCoords = false; + unsigned int maxComponents = 0; + for (std::size_t i = 0; i < scene->mNumMeshes; ++i) { + const aiMesh* assimpMesh = scene->mMeshes[i]; + if (!assimpMesh) { + continue; + } + + totalVertices += assimpMesh->mNumVertices; + if (assimpMesh->HasTextureCoords(0)) { + hasTexCoords = true; + maxComponents = std::max(maxComponents, assimpMesh->mNumUVComponents[0]); + } + } + + if (!hasTexCoords || totalVertices == 0) { + return; + } + + if (mTriMesh) { + const auto expectedVertices = mTriMesh->getVertices().size(); + if (expectedVertices != 0 && totalVertices != expectedVertices) { + return; + } + } + + mTextureCoords.reserve(totalVertices); + for (std::size_t i = 0; i < scene->mNumMeshes; ++i) { + const aiMesh* assimpMesh = scene->mMeshes[i]; + if (!assimpMesh) { + continue; + } + + const bool meshHasTexCoords = assimpMesh->HasTextureCoords(0); + for (auto j = 0u; j < assimpMesh->mNumVertices; ++j) { + if (meshHasTexCoords) { + const aiVector3D& texCoord = assimpMesh->mTextureCoords[0][j]; + mTextureCoords.emplace_back(texCoord.x, texCoord.y, texCoord.z); + } else { + mTextureCoords.emplace_back(0.0, 0.0, 0.0); + } + } + } + + mTextureCoordComponents = static_cast(std::min(maxComponents, 3u)); +} + +//============================================================================== +void MeshShape::extractMaterialsFromScene( + const aiScene* scene, + const std::string& basePath, + const common::Uri& meshUri) +{ + if (!scene || scene->mNumMaterials == 0) { + return; + } + + mMaterials.clear(); + mMaterials.reserve(scene->mNumMaterials); + + const common::filesystem::path meshPath = basePath; + std::error_code meshPathEc; + const bool meshPathExists + = !basePath.empty() && common::filesystem::exists(meshPath, meshPathEc) + && !meshPathEc; + + for (std::size_t i = 0; i < scene->mNumMaterials; ++i) { + aiMaterial* aiMat = scene->mMaterials[i]; + assert(aiMat); + + MeshMaterial material; + + // Extract colors + aiColor4D c; + if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_AMBIENT, &c) == AI_SUCCESS) { + material.ambient = Eigen::Vector4f(c.r, c.g, c.b, c.a); + } + + if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_DIFFUSE, &c) == AI_SUCCESS) { + material.diffuse = Eigen::Vector4f(c.r, c.g, c.b, c.a); + } + + if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_SPECULAR, &c) == AI_SUCCESS) { + material.specular = Eigen::Vector4f(c.r, c.g, c.b, c.a); + } + + if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_EMISSIVE, &c) == AI_SUCCESS) { + material.emissive = Eigen::Vector4f(c.r, c.g, c.b, c.a); + } + + // Extract shininess + unsigned int maxValue = 1; + float shininess = 0.0f, strength = 1.0f; + if (aiGetMaterialFloatArray( + aiMat, AI_MATKEY_SHININESS, &shininess, &maxValue) + == AI_SUCCESS) { + maxValue = 1; + if (aiGetMaterialFloatArray( + aiMat, AI_MATKEY_SHININESS_STRENGTH, &strength, &maxValue) + == AI_SUCCESS) { + shininess *= strength; + } + material.shininess = shininess; + } + + // Extract texture paths for all texture types + // Check common texture types and store them + const aiTextureType textureTypes[] + = {aiTextureType_DIFFUSE, + aiTextureType_SPECULAR, + aiTextureType_NORMALS, + aiTextureType_AMBIENT, + aiTextureType_EMISSIVE, + aiTextureType_HEIGHT, + aiTextureType_SHININESS, + aiTextureType_OPACITY, + aiTextureType_DISPLACEMENT, + aiTextureType_LIGHTMAP, + aiTextureType_REFLECTION}; + + for (const auto& type : textureTypes) { + const auto count = aiMat->GetTextureCount(type); + for (auto j = 0u; j < count; ++j) { + aiString imagePath; + if (aiMat->GetTexture(type, j, &imagePath) == AI_SUCCESS) { + const std::string imagePathString = imagePath.C_Str(); + if (imagePathString.empty()) { + continue; + } + + const common::filesystem::path relativeImagePath = imagePathString; + std::error_code ec; + bool attemptedCanonicalize = false; + if (!basePath.empty() || relativeImagePath.is_absolute()) { + const common::filesystem::path absoluteImagePath + = common::filesystem::canonical( + meshPath.parent_path() / relativeImagePath, ec); + attemptedCanonicalize = true; + if (!ec) { + material.textureImagePaths.emplace_back( + absoluteImagePath.string()); + continue; + } + } + + bool resolved = false; + if (meshUri.mPath) { + common::Uri resolvedUri; + if (resolvedUri.fromRelativeUri(meshUri, imagePathString)) { + material.textureImagePaths.emplace_back(resolvedUri.toString()); + resolved = true; + } + } + + if (!resolved) { + material.textureImagePaths.emplace_back(imagePathString); + } + + if (meshPathExists && attemptedCanonicalize && !resolved) { + DART_WARN( + "[MeshShape::extractMaterialsFromScene] Failed to resolve " + "texture image path from (base: `{}`, relative: '{}').", + meshPath.parent_path().string(), + relativeImagePath.string()); + } + } + } + } + + mMaterials.emplace_back(std::move(material)); + } +} + //============================================================================== Eigen::Matrix3d MeshShape::computeInertia(double _mass) const { @@ -366,15 +1369,22 @@ Eigen::Matrix3d MeshShape::computeInertia(double _mass) const //============================================================================== ShapePtr MeshShape::clone() const { - aiScene* new_scene = cloneMesh(); + std::shared_ptr> clonedTriMesh; + if (const auto triMesh = getTriMesh()) { + clonedTriMesh = std::make_shared>(*triMesh); + } - auto new_shape = std::make_shared( - mScale, new_scene, mMeshUri, mResourceRetriever, MeshOwnership::Copied); + auto new_shape = std::make_shared(mScale, clonedTriMesh, mMeshUri); new_shape->mMeshPath = mMeshPath; + new_shape->mResourceRetriever = mResourceRetriever; new_shape->mDisplayList = mDisplayList; new_shape->mColorMode = mColorMode; new_shape->mAlphaMode = mAlphaMode; new_shape->mColorIndex = mColorIndex; + new_shape->mMaterials = mMaterials; + new_shape->mSubMeshRanges = mSubMeshRanges; + new_shape->mTextureCoords = mTextureCoords; + new_shape->mTextureCoordComponents = mTextureCoordComponents; return new_shape; } @@ -382,7 +1392,12 @@ ShapePtr MeshShape::clone() const //============================================================================== void MeshShape::updateBoundingBox() const { - if (!mMesh) { + // Use getTriMesh() instead of directly accessing mTriMesh + // This handles lazy conversion for backward compatibility with + // CustomMeshShape + const auto* triMesh = getTriMesh().get(); + + if (!triMesh || triMesh->getVertices().empty()) { mBoundingBox.setMin(Eigen::Vector3d::Zero()); mBoundingBox.setMax(Eigen::Vector3d::Zero()); mIsBoundingBoxDirty = false; @@ -393,13 +1408,11 @@ void MeshShape::updateBoundingBox() const = Eigen::Vector3d::Constant(std::numeric_limits::infinity()); Eigen::Vector3d maxPoint = -minPoint; - for (unsigned i = 0u; i < mMesh->mNumMeshes; i++) { - for (unsigned j = 0u; j < mMesh->mMeshes[i]->mNumVertices; j++) { - const auto& vertex = mMesh->mMeshes[i]->mVertices[j]; - const Eigen::Vector3d eigenVertex(vertex.x, vertex.y, vertex.z); - minPoint = minPoint.cwiseMin(eigenVertex.cwiseProduct(mScale)); - maxPoint = maxPoint.cwiseMax(eigenVertex.cwiseProduct(mScale)); - } + const auto& vertices = triMesh->getVertices(); + for (const auto& vertex : vertices) { + const Eigen::Vector3d scaledVertex = vertex.cwiseProduct(mScale); + minPoint = minPoint.cwiseMin(scaledVertex); + maxPoint = maxPoint.cwiseMax(scaledVertex); } mBoundingBox.setMin(minPoint); @@ -419,11 +1432,14 @@ void MeshShape::updateVolume() const //============================================================================== aiScene* MeshShape::cloneMesh() const { - if (!mMesh) + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* scene = getMesh(); + DART_SUPPRESS_DEPRECATED_END + if (!scene) return nullptr; aiScene* new_scene = nullptr; - aiCopyScene(mMesh.get(), &new_scene); + aiCopyScene(scene, &new_scene); return new_scene; } @@ -500,8 +1516,18 @@ const aiScene* MeshShape::loadMesh( // Wrap ResourceRetriever in an IOSystem from Assimp's C++ API. Then wrap // the IOSystem in an aiFileIO from Assimp's C API. Yes, this API is // completely ridiculous... - AssimpInputResourceRetrieverAdaptor systemIO(retriever); - aiFileIO fileIO = createFileIO(&systemIO); + aiFileIO* fileIOPtr = nullptr; + std::optional systemIO; + std::optional fileIO; + if (retriever) { + // Suppress deprecation warnings - we need to use this for backward + // compatibility + DART_SUPPRESS_DEPRECATED_BEGIN + systemIO.emplace(retriever); + fileIO.emplace(createFileIO(&systemIO.value())); + DART_SUPPRESS_DEPRECATED_END + fileIOPtr = &fileIO.value(); + } // Import the file. const aiScene* scene = aiImportFileExWithProperties( @@ -509,7 +1535,7 @@ const aiScene* MeshShape::loadMesh( aiProcess_GenNormals | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType | aiProcess_OptimizeMeshes, - &fileIO, + fileIOPtr, propertyStore); // If succeeded, store the importer in the scene to keep it alive. This is @@ -539,14 +1565,20 @@ const aiScene* MeshShape::loadMesh( const aiScene* MeshShape::loadMesh( const common::Uri& uri, const common::ResourceRetrieverPtr& retriever) { - return loadMesh(uri.toString(), retriever); + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* scene = loadMesh(uri.toString(), retriever); + DART_SUPPRESS_DEPRECATED_END + return scene; } //============================================================================== const aiScene* MeshShape::loadMesh(const std::string& filePath) { const auto retriever = std::make_shared(); - return loadMesh("file://" + filePath, retriever); + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* scene = loadMesh("file://" + filePath, retriever); + DART_SUPPRESS_DEPRECATED_END + return scene; } } // namespace dynamics diff --git a/dart/dynamics/MeshShape.hpp b/dart/dynamics/MeshShape.hpp index 1a1149412ad86..ceb0e4503d41e 100644 --- a/dart/dynamics/MeshShape.hpp +++ b/dart/dynamics/MeshShape.hpp @@ -33,8 +33,11 @@ #ifndef DART_DYNAMICS_MESHSHAPE_HPP_ #define DART_DYNAMICS_MESHSHAPE_HPP_ +#include #include +#include + #include #include @@ -42,6 +45,7 @@ #include #include +#include namespace dart { namespace dynamics { @@ -88,8 +92,24 @@ class DART_API MeshShape : public Shape SHAPE_ALPHA }; - /// Constructor. + /// Constructor using TriMesh (preferred). + MeshShape( + const Eigen::Vector3d& scale, + std::shared_ptr> mesh, + const common::Uri& uri = ""); + + /// Constructor using TriMesh (preferred) with a resource retriever. + /// + /// Providing a resource retriever allows MeshShape to resolve and preserve + /// material/texture metadata associated with the source URI. MeshShape( + const Eigen::Vector3d& scale, + std::shared_ptr> mesh, + const common::Uri& uri, + common::ResourceRetrieverPtr resourceRetriever); + + /// Constructor using aiScene (deprecated, for backward compatibility). + [[deprecated("Use TriMesh-based APIs; Assimp APIs will be removed in DART 8.")]] MeshShape( const Eigen::Vector3d& scale, const aiScene* mesh, const common::Uri& uri = "", @@ -98,7 +118,7 @@ class DART_API MeshShape : public Shape /// Constructor that accepts a shared_ptr so callers can supply a custom /// deleter for aiScene. - MeshShape( + [[deprecated("Use TriMesh-based APIs; Assimp APIs will be removed in DART 8.")]] MeshShape( const Eigen::Vector3d& scale, std::shared_ptr mesh, const common::Uri& uri = "", @@ -113,7 +133,18 @@ class DART_API MeshShape : public Shape /// Returns shape type for this class static const std::string& getStaticType(); - const aiScene* getMesh() const; + /// Returns the TriMesh representation of this mesh (preferred). + std::shared_ptr> getTriMesh() const; + + /// Returns the aiScene representation of this mesh (deprecated). + /// + /// WARNING: This method performs an expensive conversion from TriMesh to + /// aiScene on every call. It is provided only for backward compatibility. + /// Please migrate to getTriMesh() for better performance. + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART " + "8.")]] const aiScene* + getMesh() const; /// Updates positions of the vertices or the elements. By default, this does /// nothing; you must extend the MeshShape class and implement your own @@ -121,25 +152,33 @@ class DART_API MeshShape : public Shape /// rendering virtual void update(); - void setMesh( + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART 8.")]] void + setMesh( const aiScene* mesh, const std::string& path = "", common::ResourceRetrieverPtr resourceRetriever = nullptr); /// Sets the mesh pointer with explicit ownership semantics. - void setMesh( + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART 8.")]] void + setMesh( const aiScene* mesh, MeshOwnership ownership, const common::Uri& path, common::ResourceRetrieverPtr resourceRetriever = nullptr); - void setMesh( + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART 8.")]] void + setMesh( const aiScene* mesh, const common::Uri& path, common::ResourceRetrieverPtr resourceRetriever = nullptr); /// Sets the mesh using a shared_ptr so callers can provide a custom deleter. - void setMesh( + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART 8.")]] void + setMesh( std::shared_ptr mesh, const common::Uri& path = "", common::ResourceRetrieverPtr resourceRetriever = nullptr); @@ -202,12 +241,32 @@ class DART_API MeshShape : public Shape void setDisplayList(int index); - static const aiScene* loadMesh(const std::string& filePath); + /// Returns materials extracted from the mesh (for rendering without Assimp). + /// This provides an Assimp-free way to access material properties. + const std::vector& getMaterials() const; + + /// Returns the number of materials in this mesh. + std::size_t getNumMaterials() const; + + /// Returns a specific material by index. + /// Returns nullptr if index is out of bounds. + const MeshMaterial* getMaterial(std::size_t index) const; - static const aiScene* loadMesh( + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART " + "8.")]] static const aiScene* + loadMesh(const std::string& filePath); + + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART " + "8.")]] static const aiScene* + loadMesh( const std::string& _uri, const common::ResourceRetrieverPtr& retriever); - static const aiScene* loadMesh( + [[deprecated( + "Use TriMesh-based APIs; Assimp APIs will be removed in DART " + "8.")]] static const aiScene* + loadMesh( const common::Uri& uri, const common::ResourceRetrieverPtr& retriever); // Documentation inherited. @@ -217,6 +276,42 @@ class DART_API MeshShape : public Shape virtual ShapePtr clone() const override; protected: + class DART_API MeshHandle + { + public: + MeshHandle() = default; + + MeshHandle& operator=(const aiScene* mesh); + MeshHandle& operator=(std::shared_ptr mesh); + + const aiScene* get() const; + const aiScene* operator->() const; + explicit operator bool() const; + + void reset(); + MeshOwnership getOwnership() const; + const std::shared_ptr& getShared() const; + + void set(const aiScene* mesh, MeshOwnership ownership); + void set(std::shared_ptr mesh); + + private: + std::shared_ptr mMesh; + MeshOwnership mMeshOwnership{MeshOwnership::None}; + }; + + struct SubMeshRange + { + std::size_t vertexOffset{0}; + std::size_t vertexCount{0}; + std::size_t triangleOffset{0}; + std::size_t triangleCount{0}; + unsigned int materialIndex{0}; + }; + + static std::shared_ptr makeMeshHandle( + const aiScene* mesh, MeshOwnership ownership); + // Documentation inherited. void updateBoundingBox() const override; @@ -227,45 +322,42 @@ class DART_API MeshShape : public Shape void releaseMesh(); - struct MeshPtr : public std::shared_ptr - { - // Backward-compatibility shim for downstream code (e.g., Gazebo) that - // assigns raw aiScene pointers directly to MeshShape::mMesh. - // - // Prefer using MeshShape::setMesh(..., MeshOwnership, ...) instead. - // TODO(DART 8): Remove this shim and require setMesh(). - using std::shared_ptr::shared_ptr; - using std::shared_ptr::operator=; - - MeshPtr& operator=(aiScene* scene) - { - if (!scene) { - this->reset(); - return *this; - } - - this->reset(scene, [](const aiScene* ownedScene) { - delete const_cast(ownedScene); - }); - return *this; - } - - MeshPtr& operator=(const aiScene* scene) - { - if (!scene) { - this->reset(); - return *this; - } - - this->reset(scene, [](const aiScene* importedScene) { - aiReleaseImport(importedScene); - }); - return *this; - } - }; + MeshHandle mMesh; - MeshPtr mMesh; - MeshOwnership mMeshOwnership{MeshOwnership::None}; + /// Converts aiScene to TriMesh for internal use. + static std::shared_ptr> convertAssimpMesh( + const aiScene* scene); + static std::shared_ptr> convertAssimpMesh( + const aiScene* scene, std::vector* subMeshes); + static bool collectSubMeshRanges( + const aiScene* scene, + std::vector& ranges, + std::size_t expectedVertices, + std::size_t expectedTriangles); + + /// Converts TriMesh back to aiScene for backward compatibility. + /// NOTE: This creates a new aiScene on every call and the caller is + /// responsible for freeing it with aiReleaseImport(). + const aiScene* convertToAssimpMesh() const; + + /// Extracts material information from aiScene for Assimp-free rendering. + void extractMaterialsFromScene( + const aiScene* scene, + const std::string& basePath, + const common::Uri& meshUri); + + /// Extracts texture coordinates from aiScene for Assimp-free rendering. + void extractTextureCoordsFromScene(const aiScene* scene); + + /// TriMesh representation (preferred, ownership shared). + std::shared_ptr> mTriMesh; + + /// Submesh ranges extracted from Assimp (for backward compatibility). + std::vector mSubMeshRanges; + + /// Cached aiScene for backward compatibility (created on-demand). + /// Mutable to allow lazy conversion in const getMesh() method. + mutable const aiScene* mCachedAiScene; /// URI the mesh, if available). common::Uri mMeshUri; @@ -276,6 +368,12 @@ class DART_API MeshShape : public Shape /// Optional method of loading resources by URI. common::ResourceRetrieverPtr mResourceRetriever; + /// Texture coordinates aligned with TriMesh vertex order. + std::vector mTextureCoords; + + /// Number of texture coordinate components (0 means none). + int mTextureCoordComponents{0}; + /// OpenGL DisplayList id for rendering int mDisplayList; @@ -290,6 +388,9 @@ class DART_API MeshShape : public Shape /// Specifies which color index should be used when mColorMode is COLOR_INDEX int mColorIndex; + + /// Materials extracted from the mesh (for Assimp-free rendering). + std::vector mMaterials; }; } // namespace dynamics diff --git a/dart/dynamics/SoftMeshShape.cpp b/dart/dynamics/SoftMeshShape.cpp index 28b69a8b1efe3..767cbe05250e5 100644 --- a/dart/dynamics/SoftMeshShape.cpp +++ b/dart/dynamics/SoftMeshShape.cpp @@ -41,7 +41,10 @@ namespace dart { namespace dynamics { SoftMeshShape::SoftMeshShape(SoftBodyNode* _softBodyNode) - : Shape(SOFT_MESH), mSoftBodyNode(_softBodyNode), mAssimpMesh(nullptr) + : Shape(SOFT_MESH), + mSoftBodyNode(_softBodyNode), + mTriMesh(std::make_shared>()), + mAssimpMesh(nullptr) { DART_ASSERT(_softBodyNode != nullptr); // Build mesh here using soft body node @@ -68,6 +71,12 @@ const std::string& SoftMeshShape::getStaticType() return type; } +//============================================================================== +std::shared_ptr> SoftMeshShape::getTriMesh() const +{ + return mTriMesh; +} + //============================================================================== const aiMesh* SoftMeshShape::getAssimpMesh() const { @@ -116,46 +125,61 @@ void SoftMeshShape::_buildMesh() int nVertices = mSoftBodyNode->getNumPointMasses(); int nFaces = mSoftBodyNode->getNumFaces(); - // Create new aiMesh mAssimpMesh = std::make_unique(); - // Set vertices and normals + // Reserve space for vertices and triangles + mTriMesh->reserveVertices(nVertices); + mTriMesh->reserveTriangles(nFaces); + + // Add vertices (using resting positions) mAssimpMesh->mNumVertices = nVertices; mAssimpMesh->mVertices = new aiVector3D[nVertices]; mAssimpMesh->mNormals = new aiVector3D[nVertices]; - aiVector3D itAIVector3d; + for (int i = 0; i < nVertices; ++i) { PointMass* itPointMass = mSoftBodyNode->getPointMass(i); const Eigen::Vector3d& vertex = itPointMass->getRestingPosition(); - itAIVector3d.Set(vertex[0], vertex[1], vertex[2]); - mAssimpMesh->mVertices[i] = itAIVector3d; - mAssimpMesh->mNormals[i] = itAIVector3d; + mTriMesh->addVertex(vertex); + aiVector3D aiVertex; + aiVertex.Set(vertex[0], vertex[1], vertex[2]); + mAssimpMesh->mVertices[i] = aiVertex; + mAssimpMesh->mNormals[i] = aiVertex; } - // Set faces + // Add faces (triangles) mAssimpMesh->mNumFaces = nFaces; mAssimpMesh->mFaces = new aiFace[nFaces]; + for (int i = 0; i < nFaces; ++i) { Eigen::Vector3i itFace = mSoftBodyNode->getFace(i); - aiFace* itAIFace = &mAssimpMesh->mFaces[i]; - itAIFace->mNumIndices = 3; - itAIFace->mIndices = new unsigned int[3]; - itAIFace->mIndices[0] = itFace[0]; - itAIFace->mIndices[1] = itFace[1]; - itAIFace->mIndices[2] = itFace[2]; + mTriMesh->addTriangle( + math::TriMesh::Triangle(itFace[0], itFace[1], itFace[2])); + + aiFace* aiFacePtr = &mAssimpMesh->mFaces[i]; + aiFacePtr->mNumIndices = 3; + aiFacePtr->mIndices = new unsigned int[3]; + aiFacePtr->mIndices[0] = itFace[0]; + aiFacePtr->mIndices[1] = itFace[1]; + aiFacePtr->mIndices[2] = itFace[2]; } } void SoftMeshShape::update() { + // Update vertex positions from soft body node std::size_t nVertices = mSoftBodyNode->getNumPointMasses(); - aiVector3D itAIVector3d; + // Get non-const access to vertices for in-place update + auto& vertices = mTriMesh->getVertices(); + for (std::size_t i = 0; i < nVertices; ++i) { PointMass* itPointMass = mSoftBodyNode->getPointMass(i); - const Eigen::Vector3d& vertex = itPointMass->getLocalPosition(); - itAIVector3d.Set(vertex[0], vertex[1], vertex[2]); - mAssimpMesh->mVertices[i] = itAIVector3d; + vertices[i] = itPointMass->getLocalPosition(); + + if (mAssimpMesh && mAssimpMesh->mVertices) { + const auto& vertex = vertices[i]; + mAssimpMesh->mVertices[i].Set(vertex[0], vertex[1], vertex[2]); + } } } diff --git a/dart/dynamics/SoftMeshShape.hpp b/dart/dynamics/SoftMeshShape.hpp index 1d9d070ee5e44..aa03dba14d34d 100644 --- a/dart/dynamics/SoftMeshShape.hpp +++ b/dart/dynamics/SoftMeshShape.hpp @@ -35,9 +35,13 @@ #include +#include + #include #include +#include + namespace dart { namespace dynamics { @@ -61,8 +65,15 @@ class DART_API SoftMeshShape : public Shape /// Returns shape type for this class static const std::string& getStaticType(); - /// @brief - const aiMesh* getAssimpMesh() const; + /// Returns the TriMesh representation of this soft mesh. + std::shared_ptr> getTriMesh() const; + + /// Returns the aiMesh representation (deprecated, for backward + /// compatibility). + [[deprecated( + "Use getTriMesh() instead; Assimp APIs will be removed in DART " + "8.")]] const aiMesh* + getAssimpMesh() const; /// Get the SoftBodyNode that is associated with this SoftMeshShape const SoftBodyNode* getSoftBodyNode() const; @@ -90,7 +101,10 @@ class DART_API SoftMeshShape : public Shape /// @brief SoftBodyNode* mSoftBodyNode; - /// @brief + /// TriMesh representation (preferred). + std::shared_ptr> mTriMesh; + + /// Cached aiMesh representation (deprecated, for backward compatibility). std::unique_ptr mAssimpMesh; }; diff --git a/dart/dynamics/AssimpInputResourceAdaptor.cpp b/dart/dynamics/detail/AssimpInputResourceAdaptor.cpp similarity index 96% rename from dart/dynamics/AssimpInputResourceAdaptor.cpp rename to dart/dynamics/detail/AssimpInputResourceAdaptor.cpp index 3bd48c71a1f26..9cc647bb98afe 100644 --- a/dart/dynamics/AssimpInputResourceAdaptor.cpp +++ b/dart/dynamics/detail/AssimpInputResourceAdaptor.cpp @@ -30,15 +30,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/dynamics/AssimpInputResourceAdaptor.hpp" +#include "dart/dynamics/detail/AssimpInputResourceAdaptor.hpp" +#include "dart/common/Diagnostics.hpp" #include "dart/common/Logging.hpp" #include "dart/common/Macros.hpp" #include -#include - #include namespace dart { @@ -88,10 +87,14 @@ Assimp::IOStream* AssimpInputResourceRetrieverAdaptor::Open( return nullptr; } - if (const common::ResourcePtr resource = mResourceRetriever->retrieve(pFile)) + if (const common::ResourcePtr resource + = mResourceRetriever->retrieve(pFile)) { + DART_SUPPRESS_DEPRECATED_BEGIN return new AssimpInputResourceAdaptor(resource); - else + DART_SUPPRESS_DEPRECATED_END + } else { return nullptr; + } } //============================================================================== diff --git a/dart/dynamics/detail/AssimpInputResourceAdaptor.hpp b/dart/dynamics/detail/AssimpInputResourceAdaptor.hpp new file mode 100644 index 0000000000000..fc4e3a9a18c28 --- /dev/null +++ b/dart/dynamics/detail/AssimpInputResourceAdaptor.hpp @@ -0,0 +1,131 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_HPP_ +#define DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_HPP_ + +#include +#include + +#include + +#include +#include +#include + +namespace dart { +namespace dynamics { + +class DART_API AssimpInputResourceRetrieverAdaptor : public Assimp::IOSystem +{ +public: + explicit AssimpInputResourceRetrieverAdaptor( + const common::ResourceRetrieverPtr& _resourceRetriever); + virtual ~AssimpInputResourceRetrieverAdaptor(); + + /// @brief Tests for the existence of a file at the given path. + /// + /// @param pFile Path to the file + /// @return true if there is a file with this path, else false. + bool Exists(const char* pFile) const override; + + /// @brief Returns the system specific directory separator + /// + /// @return System specific directory separator + char getOsSeparator() const override; + + /// @brief Open a new file with a given path. + /// + /// When the access to the file is finished, call Close() to release + /// all associated resources (or the virtual dtor of the IOStream). + /// + /// @param pFile Path to the file + /// @param pMode Desired file I/O mode. Required are: "wb", "w", "wt", + /// "rb", "r", "rt". + /// @return New IOStream interface allowing the lib to access + /// the underlying file. + Assimp::IOStream* Open(const char* pFile, const char* pMode = "rb") override; + + /// @brief Closes the given file and releases all resources + /// associated with it. + /// @param pFile The file instance previously created by Open(). + void Close(Assimp::IOStream* pFile) override; + +private: + common::ResourceRetrieverPtr mResourceRetriever; +}; + +class DART_API AssimpInputResourceAdaptor : public Assimp::IOStream +{ +public: + explicit AssimpInputResourceAdaptor(const common::ResourcePtr& _resource); + virtual ~AssimpInputResourceAdaptor(); + + /// @brief Read from the file + /// + /// See fread() for more details + std::size_t Read( + void* pvBuffer, std::size_t pSize, std::size_t pCount) override; + + /// @brief Not implemented. This is a read-only stream. + std::size_t Write( + const void* pvBuffer, std::size_t pSize, std::size_t pCount) override; + + /// @brief Set the read/write cursor of the file + /// + /// Note that the offset is _negative_ for aiOrigin_END. + /// See fseek() for more details + aiReturn Seek(std::size_t pOffset, aiOrigin pOrigin) override; + + /// @brief Get the current position of the read/write cursor + /// + /// See ftell() for more details + std::size_t Tell() const override; + + /// @brief Returns filesize + /// + /// Returns the filesize. + std::size_t FileSize() const override; + + /// @brief Not implemented. This is a read-only stream. + void Flush() override; + +private: + common::ResourcePtr mResource; +}; + +DART_API aiFileIO createFileIO(Assimp::IOSystem* adaptor); + +} // namespace dynamics +} // namespace dart + +#endif // ifndef DART_DYNAMICS_ASSIMPINPUTRESOURCEADAPTOR_HPP_ diff --git a/dart/gui/ImGuiViewer.cpp b/dart/gui/ImGuiViewer.cpp index a9196d1085998..67a31a5b574b5 100644 --- a/dart/gui/ImGuiViewer.cpp +++ b/dart/gui/ImGuiViewer.cpp @@ -82,6 +82,13 @@ void ImGuiViewer::setImGuiScale(float scale) //============================================================================== void ImGuiViewer::setUpViewInWindow(int x, int y, int width, int height) +{ + setUpViewInWindow(x, y, width, height, 0); +} + +//============================================================================== +void ImGuiViewer::setUpViewInWindow( + int x, int y, int width, int height, int screenNum) { const float scale = mImGuiScale; int scaledWidth = width; @@ -92,7 +99,7 @@ void ImGuiViewer::setUpViewInWindow(int x, int y, int width, int height) scaledHeight = std::max(1, static_cast(std::lround(height * scale))); } - Viewer::setUpViewInWindow(x, y, scaledWidth, scaledHeight); + Viewer::setUpViewInWindow(x, y, scaledWidth, scaledHeight, screenNum); } //============================================================================== diff --git a/dart/gui/ImGuiViewer.hpp b/dart/gui/ImGuiViewer.hpp index 94ff5e93ed321..1ee3440c596b4 100644 --- a/dart/gui/ImGuiViewer.hpp +++ b/dart/gui/ImGuiViewer.hpp @@ -65,11 +65,13 @@ class DART_GUI_API ImGuiViewer : public Viewer /// Set the ImGui global scale factor (fonts + widget sizes). void setImGuiScale(float scale); - using Viewer::setUpViewInWindow; - /// Set up the viewer window, scaling width/height by the ImGui scale. void setUpViewInWindow(int x, int y, int width, int height); + /// Set up the viewer window on a specific screen, scaling width/height by the + /// ImGui scale. + void setUpViewInWindow(int x, int y, int width, int height, int screenNum); + /// Show About widget. void showAbout(); diff --git a/dart/gui/InteractiveFrame.cpp b/dart/gui/InteractiveFrame.cpp index a332c474e2aaa..f35e53be608c1 100644 --- a/dart/gui/InteractiveFrame.cpp +++ b/dart/gui/InteractiveFrame.cpp @@ -296,162 +296,114 @@ void InteractiveFrame::createStandardVisualizationShapes( p.mHeadLengthScale = 0.4; p.mDoubleArrow = false; - mTools[InteractiveTool::LINEAR][a]->addShapeFrame(dart::dynamics::ShapePtr( - new dart::dynamics::ArrowShape(tail, head, p, color, 100))); + auto arrow = std::make_shared( + tail, head, p, color, 100); + arrow->setColorMode(dart::dynamics::MeshShape::SHAPE_COLOR); + auto arrowFrame = mTools[InteractiveTool::LINEAR][a]->addShapeFrame(arrow); + arrowFrame->getVisualAspect(true)->setRGBA(color); tail[a] = -ring_inner_scale; head[a] = -size; - mTools[InteractiveTool::LINEAR][a]->addShapeFrame(dart::dynamics::ShapePtr( - new dart::dynamics::ArrowShape(tail, head, p, color, 100))); + arrow = std::make_shared( + tail, head, p, color, 100); + arrow->setColorMode(dart::dynamics::MeshShape::SHAPE_COLOR); + arrowFrame = mTools[InteractiveTool::LINEAR][a]->addShapeFrame(arrow); + arrowFrame->getVisualAspect(true)->setRGBA(color); } - // Create rotation rings + // Create rotation rings - Generate TriMesh directly for (std::size_t r = 0; r < 3; ++r) { - aiMesh* mesh = new aiMesh; - mesh->mMaterialIndex = (unsigned int)(-1); + auto triMesh = std::make_shared>(); std::size_t numVertices = 8 * resolution; std::size_t R = 4 * resolution; - mesh->mNumVertices = numVertices; - mesh->mVertices = new aiVector3D[numVertices]; - mesh->mNormals = new aiVector3D[numVertices]; - mesh->mColors[0] = new aiColor4D[numVertices]; - aiVector3D vertex; - aiVector3D normal; - aiColor4D color1; - aiColor4D color2; + std::size_t numFaces = 4 * resolution; + std::size_t H = resolution / 2; + + triMesh->reserveVertices(numVertices); + triMesh->reserveTriangles(numFaces); + + // Generate vertices for (std::size_t j = 0; j < 2; ++j) { for (std::size_t i = 0; i < resolution; ++i) { double theta = (double)(i) / (double)(resolution)*2 * pi; + // Inner ring vertices double x = 0; double y = ring_inner_scale * cos(theta); double z = ring_inner_scale * sin(theta); - vertex.Set(x, y, z); - mesh->mVertices[4 * i + j] = vertex; // Front + triMesh->addVertex(Eigen::Vector3d(x, y, z)); // Front: index 4*i + j - mesh->mVertices[4 * i + j + R] = vertex; // Back + // Outer ring vertices + y = ring_outer_scale * cos(theta); + z = ring_outer_scale * sin(theta); + triMesh->addVertex( + Eigen::Vector3d(x, y, z)); // Front: index 4*i + 2 + j + } + } + // Add back face vertices (duplicate of front, for proper rendering) + for (std::size_t j = 0; j < 2; ++j) { + for (std::size_t i = 0; i < resolution; ++i) { + double theta = (double)(i) / (double)(resolution)*2 * pi; + + // Inner ring vertices (back) + double x = 0; + double y = ring_inner_scale * cos(theta); + double z = ring_inner_scale * sin(theta); + triMesh->addVertex(Eigen::Vector3d(x, y, z)); // Back: index 4*i + j + R + + // Outer ring vertices (back) y = ring_outer_scale * cos(theta); z = ring_outer_scale * sin(theta); - vertex.Set(x, y, z); - mesh->mVertices[4 * i + 2 + j] = vertex; // Front - - mesh->mVertices[4 * i + 2 + j + R] = vertex; // Back - - normal.Set(1.0f, 0.0f, 0.0f); - mesh->mNormals[4 * i + j] = normal; - mesh->mNormals[4 * i + 2 + j] = normal; - - normal.Set(-1.0f, 0.0f, 0.0f); - mesh->mNormals[4 * i + j + R] = normal; - mesh->mNormals[4 * i + 2 + j + R] = normal; - - for (std::size_t c = 0; c < 3; ++c) { - color1[c] = 0.0; - color2[c] = 0.0; - } - color1[r] = 1.0; - color2[r] = 0.6; - color1.a = getTool(InteractiveTool::ANGULAR, r)->getDefaultAlpha(); - color2.a = getTool(InteractiveTool::ANGULAR, r)->getDefaultAlpha(); - mesh->mColors[0][4 * i + j] = ((4 * i + j) % 2 == 0) ? color1 : color2; - mesh->mColors[0][4 * i + j + R] - = ((4 * i + j + R) % 2 == 0) ? color1 : color2; - mesh->mColors[0][4 * i + 2 + j] - = ((4 * i + 2 + j) % 2 == 0) ? color1 : color2; - mesh->mColors[0][4 * i + 2 + j + R] - = ((4 * i + 2 + j + R) % 2 == 0) ? color1 : color2; + triMesh->addVertex( + Eigen::Vector3d(x, y, z)); // Back: index 4*i + 2 + j + R } } - std::size_t numFaces = 4 * resolution; - std::size_t F = 2 * resolution; - std::size_t H = resolution / 2; - mesh->mNumFaces = numFaces; - mesh->mFaces = new aiFace[numFaces]; + // Generate triangles for (std::size_t i = 0; i < H; ++i) { - // Front - aiFace* face = &mesh->mFaces[2 * i]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i; - face->mIndices[1] = 8 * i + 2; - face->mIndices[2] = (i + 1 < H) ? 8 * i + 6 : 2; - - face = &mesh->mFaces[2 * i + 2 * H]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i; - face->mIndices[1] = (i + 1 < H) ? 8 * i + 6 : 2; - face->mIndices[2] = (i + 1 < H) ? 8 * i + 4 : 0; - - // Back - face = &mesh->mFaces[2 * i + F]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i + R; - face->mIndices[1] = (i + 1 < H) ? 8 * i + 6 + R : 2 + R; - face->mIndices[2] = 8 * i + 2 + R; - - face = &mesh->mFaces[2 * i + 2 * H + F]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i + R; - face->mIndices[1] = (i + 1 < H) ? 8 * i + 4 + R : 0; - face->mIndices[2] = (i + 1 < H) ? 8 * i + 6 + R : 2; - - // Front - face = &mesh->mFaces[2 * i + 1]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i + 5; - face->mIndices[1] = 8 * i + 7; - face->mIndices[2] = (i + 1 < H) ? 8 * i + 11 : 3; - - face = &mesh->mFaces[2 * i + 1 + 2 * H]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i + 5; - face->mIndices[1] = (i + 1 < H) ? 8 * i + 11 : 3; - face->mIndices[2] = (i + 1 < H) ? 8 * i + 9 : 1; - - // Back - face = &mesh->mFaces[2 * i + 1 + F]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i + 5 + R; - face->mIndices[1] = (i + 1 < H) ? 8 * i + 11 + R : 3 + R; - face->mIndices[2] = 8 * i + 7 + R; - - face = &mesh->mFaces[2 * i + 1 + 2 * H + F]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - face->mIndices[0] = 8 * i + 5 + R; - face->mIndices[1] = (i + 1 < H) ? 8 * i + 9 + R : 1 + R; - face->mIndices[2] = (i + 1 < H) ? 8 * i + 11 + R : 3 + R; - } + // Front faces + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i, 8 * i + 2, (i + 1 < H) ? 8 * i + 6 : 2)); - aiNode* node = new aiNode; - node->mNumMeshes = 1; - node->mMeshes = new unsigned int[1]; - node->mMeshes[0] = 0; + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i, (i + 1 < H) ? 8 * i + 6 : 2, (i + 1 < H) ? 8 * i + 4 : 0)); - aiScene* scene = new aiScene; - scene->mNumMeshes = 1; - scene->mMeshes = new aiMesh*[1]; - scene->mMeshes[0] = mesh; - scene->mRootNode = node; + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i + 5, 8 * i + 7, (i + 1 < H) ? 8 * i + 11 : 3)); + + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i + 5, + (i + 1 < H) ? 8 * i + 11 : 3, + (i + 1 < H) ? 8 * i + 9 : 1)); + + // Back faces + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i + R, (i + 1 < H) ? 8 * i + 6 + R : 2 + R, 8 * i + 2 + R)); + + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i + R, + (i + 1 < H) ? 8 * i + 4 + R : 0, + (i + 1 < H) ? 8 * i + 6 + R : 2)); + + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i + 5 + R, (i + 1 < H) ? 8 * i + 11 + R : 3 + R, 8 * i + 7 + R)); + + triMesh->addTriangle(dart::math::TriMesh::Triangle( + 8 * i + 5 + R, + (i + 1 < H) ? 8 * i + 9 + R : 1 + R, + (i + 1 < H) ? 8 * i + 11 + R : 3 + R)); + } std::shared_ptr shape( - new dart::dynamics::MeshShape( - Eigen::Vector3d::Ones(), - scene, - common::Uri(), - nullptr, - dart::dynamics::MeshShape::MeshOwnership::Manual)); - shape->setColorMode(dart::dynamics::MeshShape::COLOR_INDEX); + new dart::dynamics::MeshShape(Eigen::Vector3d::Ones(), triMesh)); + shape->setColorMode(dart::dynamics::MeshShape::SHAPE_COLOR); + + Eigen::Vector4d color(Eigen::Vector4d::Zero()); + color[r] = 1.0; + color[3] = getTool(InteractiveTool::ANGULAR, r)->getDefaultAlpha(); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); if (r == 1) @@ -461,86 +413,47 @@ void InteractiveFrame::createStandardVisualizationShapes( auto shapeFrame = mTools[InteractiveTool::ANGULAR][r]->addShapeFrame(shape); shapeFrame->setRelativeTransform(tf); + shapeFrame->getVisualAspect(true)->setRGBA(color); } - // Create translation planes + // Create translation planes - Generate TriMesh directly for (std::size_t p = 0; p < 3; ++p) { - aiMesh* mesh = new aiMesh; - mesh->mMaterialIndex = (unsigned int)(-1); - - std::size_t numVertices = 8; - mesh->mNumVertices = numVertices; - mesh->mVertices = new aiVector3D[numVertices]; - mesh->mNormals = new aiVector3D[numVertices]; - mesh->mColors[0] = new aiColor4D[numVertices]; + auto triMesh = std::make_shared>(); double L = plane_length; - for (std::size_t i = 0; i < 2; ++i) { - mesh->mVertices[4 * i + 0] = aiVector3D(0, -L, -L); - mesh->mVertices[4 * i + 1] = aiVector3D(0, L, -L); - mesh->mVertices[4 * i + 2] = aiVector3D(0, -L, L); - mesh->mVertices[4 * i + 3] = aiVector3D(0, L, L); - } - for (std::size_t i = 0; i < 4; ++i) { - mesh->mNormals[i] = aiVector3D(1, 0, 0); - mesh->mNormals[i + 4] = aiVector3D(-1, 0, 0); - } + // Add 8 vertices (2 quads for front and back faces) + triMesh->reserveVertices(8); + triMesh->reserveTriangles(4); - aiColor4D color( - 0.1, 0.1, 0.1, getTool(InteractiveTool::PLANAR, p)->getDefaultAlpha()); - color[p] = 0.9; - for (std::size_t i = 0; i < numVertices; ++i) - mesh->mColors[0][i] = color; - - std::size_t numFaces = 4; - mesh->mNumFaces = numFaces; - mesh->mFaces = new aiFace[numFaces]; - for (std::size_t i = 0; i < numFaces; ++i) { - aiFace* face = &mesh->mFaces[i]; - face->mNumIndices = 3; - face->mIndices = new unsigned int[3]; - } + // Front face vertices (indices 0-3) + triMesh->addVertex(Eigen::Vector3d(0, -L, -L)); // 0 + triMesh->addVertex(Eigen::Vector3d(0, L, -L)); // 1 + triMesh->addVertex(Eigen::Vector3d(0, -L, L)); // 2 + triMesh->addVertex(Eigen::Vector3d(0, L, L)); // 3 + + // Back face vertices (indices 4-7) + triMesh->addVertex(Eigen::Vector3d(0, -L, -L)); // 4 + triMesh->addVertex(Eigen::Vector3d(0, L, -L)); // 5 + triMesh->addVertex(Eigen::Vector3d(0, -L, L)); // 6 + triMesh->addVertex(Eigen::Vector3d(0, L, L)); // 7 - aiFace* face = &mesh->mFaces[0]; - face->mIndices[0] = 0; - face->mIndices[1] = 1; - face->mIndices[2] = 3; - - face = &mesh->mFaces[1]; - face->mIndices[0] = 0; - face->mIndices[1] = 3; - face->mIndices[2] = 2; - - face = &mesh->mFaces[2]; - face->mIndices[0] = 4; - face->mIndices[1] = 7; - face->mIndices[2] = 5; - - face = &mesh->mFaces[3]; - face->mIndices[0] = 4; - face->mIndices[1] = 6; - face->mIndices[2] = 7; - - aiNode* node = new aiNode; - node->mNumMeshes = 1; - node->mMeshes = new unsigned int[1]; - node->mMeshes[0] = 0; - - aiScene* scene = new aiScene; - scene->mNumMeshes = 1; - scene->mMeshes = new aiMesh*[1]; - scene->mMeshes[0] = mesh; - scene->mRootNode = node; + // Add triangles (2 per face, 4 total) + // Front face + triMesh->addTriangle(dart::math::TriMesh::Triangle(0, 1, 3)); + triMesh->addTriangle(dart::math::TriMesh::Triangle(0, 3, 2)); + + // Back face + triMesh->addTriangle(dart::math::TriMesh::Triangle(4, 7, 5)); + triMesh->addTriangle(dart::math::TriMesh::Triangle(4, 6, 7)); std::shared_ptr shape( - new dart::dynamics::MeshShape( - Eigen::Vector3d::Ones(), - scene, - common::Uri(), - nullptr, - dart::dynamics::MeshShape::MeshOwnership::Manual)); - shape->setColorMode(dart::dynamics::MeshShape::COLOR_INDEX); + new dart::dynamics::MeshShape(Eigen::Vector3d::Ones(), triMesh)); + shape->setColorMode(dart::dynamics::MeshShape::SHAPE_COLOR); + + Eigen::Vector4d color( + 0.1, 0.1, 0.1, getTool(InteractiveTool::PLANAR, p)->getDefaultAlpha()); + color[p] = 0.9; Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); if (p == 1) @@ -550,6 +463,7 @@ void InteractiveFrame::createStandardVisualizationShapes( auto shapeFrame = mTools[InteractiveTool::PLANAR][p]->addShapeFrame(shape); shapeFrame->setRelativeTransform(tf); + shapeFrame->getVisualAspect(true)->setRGBA(color); } for (std::size_t i = 0; i < InteractiveTool::NUM_TYPES; ++i) { diff --git a/dart/gui/render/MeshShapeNode.cpp b/dart/gui/render/MeshShapeNode.cpp index 4c3cc6d54e9bd..5d0fbc6855085 100644 --- a/dart/gui/render/MeshShapeNode.cpp +++ b/dart/gui/render/MeshShapeNode.cpp @@ -32,14 +32,18 @@ #include "dart/gui/render/MeshShapeNode.hpp" +#include "dart/common/Diagnostics.hpp" #include "dart/common/Filesystem.hpp" #include "dart/common/Logging.hpp" #include "dart/common/Macros.hpp" #include "dart/common/Uri.hpp" +#include "dart/dynamics/InvalidIndex.hpp" +#include "dart/dynamics/MeshMaterial.hpp" #include "dart/dynamics/MeshShape.hpp" #include "dart/dynamics/SimpleFrame.hpp" #include "dart/gui/Utils.hpp" +#include #include #include #include @@ -64,39 +68,6 @@ namespace render { namespace { -//============================================================================== -#define GET_TEXTURE_TYPE_AND_COUNT(MATERIAL, TYPE) \ - { \ - const auto count = MATERIAL.GetTextureCount(TYPE); \ - if (count) \ - return std::make_pair(TYPE, count); \ - } - -//============================================================================== -std::pair getTextureTypeAndCount( - const aiMaterial& material) -{ - // For now, only single texture is supported. So we only checks whether the - // texture counter is non-zero. - - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_NONE) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_DIFFUSE) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_SPECULAR) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_AMBIENT) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_EMISSIVE) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_HEIGHT) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_NORMALS) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_SHININESS) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_OPACITY) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_DISPLACEMENT) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_LIGHTMAP) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_REFLECTION) - GET_TEXTURE_TYPE_AND_COUNT(material, aiTextureType_UNKNOWN) - - // This shouldn't be reached but put - return std::make_pair(aiTextureType_UNKNOWN, 0u); -} - //============================================================================== bool isTransparent(const ::osg::Material* material) { @@ -329,7 +300,10 @@ class MeshShapeGeometry : public ShapeNode, public ::osg::Geometry MeshShapeNode::MeshShapeNode( std::shared_ptr shape, ShapeFrameNode* parentNode) - : ShapeNode(shape, parentNode, this), mMeshShape(shape), mRootAiNode(nullptr) + : ShapeNode(shape, parentNode, this), + mMeshShape(shape), + mRootAiNode(nullptr), + mMaterialVersion(dart::dynamics::INVALID_INDEX) { extractData(true); setNodeMask(mVisualAspect->isHidden() ? 0x0 : ~0x0); @@ -360,136 +334,96 @@ bool checkSpecularSanity(const aiColor4D& c) //============================================================================== void MeshShapeNode::extractData(bool firstTime) { + // Use deprecated getMesh() for now to maintain backward compatibility + // with the scene graph structure (aiScene contains node hierarchy) + // TODO: Future work - refactor to use TriMesh directly with a scene graph + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* scene = mMeshShape->getMesh(); + DART_SUPPRESS_DEPRECATED_END const aiNode* root = scene->mRootNode; - if (firstTime) // extract material properties + const bool updateMaterials + = firstTime + || (mShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) + && mMaterialVersion != mMeshShape->getVersion()); + + if (updateMaterials) // extract material properties from MeshShape + // (Assimp-free) { clearTemporaryTextures(); mMaterials.clear(); mTextureImageArrays.clear(); - mMaterials.reserve(scene->mNumMaterials); - mTextureImageArrays.reserve(scene->mNumMaterials); + const auto& meshMaterials = mMeshShape->getMaterials(); + mMaterials.reserve(meshMaterials.size()); + mTextureImageArrays.reserve(meshMaterials.size()); const auto retriever = mMeshShape->getResourceRetriever(); const std::string meshUriString = mMeshShape->getMeshUri(); common::Uri meshUri; const bool hasMeshUri = !meshUriString.empty() && meshUri.fromStringOrPath(meshUriString); - const common::filesystem::path meshPath = mMeshShape->getMeshPath(); - const bool hasMeshFilesystemPath = !meshPath.empty(); - - for (std::size_t i = 0; i < scene->mNumMaterials; ++i) { - aiMaterial* aiMat = scene->mMaterials[i]; - DART_ASSERT(aiMat); + const common::Uri* meshUriPtr = hasMeshUri ? &meshUri : nullptr; + for (const auto& meshMat : meshMaterials) { ::osg::ref_ptr<::osg::Material> material = new ::osg::Material; - aiColor4D c; - if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_AMBIENT, &c) - == AI_SUCCESS) { - material->setAmbient( - ::osg::Material::FRONT_AND_BACK, ::osg::Vec4(c.r, c.g, c.b, c.a)); + // Convert from MeshMaterial to OSG Material + material->setAmbient( + ::osg::Material::FRONT_AND_BACK, + ::osg::Vec4( + meshMat.ambient[0], + meshMat.ambient[1], + meshMat.ambient[2], + meshMat.ambient[3])); + + material->setDiffuse( + ::osg::Material::FRONT_AND_BACK, + ::osg::Vec4( + meshMat.diffuse[0], + meshMat.diffuse[1], + meshMat.diffuse[2], + meshMat.diffuse[3])); + + // Check specular sanity + const bool specularSane + = !(meshMat.specular[0] >= 1.0f && meshMat.specular[1] >= 1.0f + && meshMat.specular[2] >= 1.0f && meshMat.specular[3] >= 1.0f); + if (specularSane) { + material->setSpecular( + ::osg::Material::FRONT_AND_BACK, + ::osg::Vec4( + meshMat.specular[0], + meshMat.specular[1], + meshMat.specular[2], + meshMat.specular[3])); } - if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_DIFFUSE, &c) - == AI_SUCCESS) { - material->setDiffuse( - ::osg::Material::FRONT_AND_BACK, ::osg::Vec4(c.r, c.g, c.b, c.a)); - } + material->setEmission( + ::osg::Material::FRONT_AND_BACK, + ::osg::Vec4( + meshMat.emissive[0], + meshMat.emissive[1], + meshMat.emissive[2], + meshMat.emissive[3])); - if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_SPECULAR, &c) - == AI_SUCCESS) { - // Some files have insane specular vectors like [1.0, 1.0, 1.0, 1.0], so - // we weed those out here - if (checkSpecularSanity(c)) - material->setSpecular( - ::osg::Material::FRONT_AND_BACK, ::osg::Vec4(c.r, c.g, c.b, c.a)); - } - - if (aiGetMaterialColor(aiMat, AI_MATKEY_COLOR_EMISSIVE, &c) - == AI_SUCCESS) { - material->setEmission( - ::osg::Material::FRONT_AND_BACK, ::osg::Vec4(c.r, c.g, c.b, c.a)); - } - - unsigned int maxValue = 1; - float shininess = 0.0f, strength = 1.0f; - if (aiGetMaterialFloatArray( - aiMat, AI_MATKEY_SHININESS, &shininess, &maxValue) - == AI_SUCCESS) { - maxValue = 1; - if (aiGetMaterialFloatArray( - aiMat, AI_MATKEY_SHININESS_STRENGTH, &strength, &maxValue) - == AI_SUCCESS) - shininess *= strength; - material->setShininess(::osg::Material::FRONT_AND_BACK, shininess); - } else { - material->setShininess(::osg::Material::FRONT_AND_BACK, 0.0f); - } + material->setShininess( + ::osg::Material::FRONT_AND_BACK, meshMat.shininess); mMaterials.push_back(material); - // Parse texture image paths - const auto textureTypeAndCount = getTextureTypeAndCount(*aiMat); - const auto& type = textureTypeAndCount.first; - const auto& count = textureTypeAndCount.second; - std::vector textureImageArray; - textureImageArray.reserve(count); + textureImageArray.reserve(meshMat.textureImagePaths.size()); - aiString imagePath; - for (auto j = 0u; j < count; ++j) { - if ((textureTypeAndCount.first == aiTextureType_NONE) - || (textureTypeAndCount.first == aiTextureType_UNKNOWN)) { - textureImageArray.emplace_back(""); - } else { - aiMat->GetTexture(type, j, &imagePath); - const common::filesystem::path relativeImagePath = imagePath.C_Str(); - std::string resolvedTexturePath; - bool attemptedFilesystemResolution = false; - - if (hasMeshFilesystemPath) { - attemptedFilesystemResolution = true; - std::error_code ec; - const auto absoluteImagePath = common::filesystem::canonical( - meshPath.parent_path() / relativeImagePath, ec); - if (!ec) - resolvedTexturePath = absoluteImagePath.string(); - } - - if (resolvedTexturePath.empty()) { - resolvedTexturePath = materializeTextureImage( - imagePath.C_Str(), - hasMeshUri ? &meshUri : nullptr, - retriever, - mTemporaryTextureFiles); - } - - if (resolvedTexturePath.empty()) { - if (attemptedFilesystemResolution) { - DART_WARN( - "[MeshShapeNode] Failed to resolve a texture image from base " - "`{}` and relative path '{}'.", - meshPath.parent_path(), - relativeImagePath); - } else { - DART_WARN( - "[MeshShapeNode] Failed to resolve a texture image '{}' for " - "mesh URI '{}'.", - imagePath.C_Str(), - meshUriString); - } - textureImageArray.emplace_back(""); - } else { - textureImageArray.emplace_back(std::move(resolvedTexturePath)); - } - } + for (const auto& texturePath : meshMat.textureImagePaths) { + textureImageArray.emplace_back(materializeTextureImage( + texturePath, meshUriPtr, retriever, mTemporaryTextureFiles)); } mTextureImageArrays.emplace_back(std::move(textureImageArray)); } + + mMaterialVersion = mMeshShape->getVersion(); } if (mShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_TRANSFORM) @@ -529,18 +463,9 @@ ::osg::Material* MeshShapeNode::getMaterial(std::size_t index) const if (index < mMaterials.size()) return mMaterials[index]; - if (!mMaterials.empty()) { - DART_WARN( - "Attempting to access material #{}, but materials only go up to {}", - index, - index - 1); - } else { - DART_WARN( - "Attempting to access material #{}, but there are no materials " - "available", - index); - } - + // Silently return nullptr when materials don't exist - this is expected for + // many mesh formats (e.g., STL, plain OBJ) and robotics meshes without + // embedded materials. The renderer will use default materials. return nullptr; } @@ -557,19 +482,8 @@ std::vector MeshShapeNode::getTextureImagePaths( if (index == std::numeric_limits::max()) return {}; - if (!mTextureImageArrays.empty()) { - DART_WARN( - "Attempting to access texture image set #{}, but materials only go up " - "to {}", - index, - index - 1); - } else { - DART_WARN( - "Attempting to access texture image set #{}, but there are no " - "materials available", - index); - } - + // Silently return empty vector when textures don't exist - this is expected + // for meshes without embedded materials return std::vector(); } @@ -710,7 +624,9 @@ void MeshShapeGeode::extractData(bool) { clearChildUtilizationFlags(); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* scene = mMeshShape->getMesh(); + DART_SUPPRESS_DEPRECATED_END for (std::size_t i = 0; i < mAiNode->mNumMeshes; ++i) { aiMesh* mesh = scene->mMeshes[mAiNode->mMeshes[i]]; @@ -934,9 +850,19 @@ void MeshShapeGeometry::extractData(bool firstTime) != static_cast( -1)) // -1 is being used by us to indicate no material { - isColored = true; ::osg::Material* material = mMainNode->getMaterial(matIndex); - if (mMeshShape->getAlphaMode() == dynamics::MeshShape::SHAPE_ALPHA) { + + // Check if material exists - if not, fall back to shape color. + if (!material) { + ss->removeAttribute(::osg::StateAttribute::MATERIAL); + ss->setMode(GL_BLEND, ::osg::StateAttribute::OFF); + ss->setRenderingHint(::osg::StateSet::OPAQUE_BIN); + ::osg::ref_ptr<::osg::Depth> depth = new ::osg::Depth; + depth->setWriteMask(true); + ss->setAttributeAndModes(depth, ::osg::StateAttribute::ON); + } else if ( + mMeshShape->getAlphaMode() == dynamics::MeshShape::SHAPE_ALPHA) { + isColored = true; const float shapeAlpha = static_cast(mVisualAspect->getAlpha()); @@ -961,6 +887,7 @@ void MeshShapeGeometry::extractData(bool firstTime) ss->setAttributeAndModes(depth, ::osg::StateAttribute::ON); } } else if (mMeshShape->getAlphaMode() == dynamics::MeshShape::BLEND) { + isColored = true; float shapeAlpha = static_cast(mVisualAspect->getAlpha()); ::osg::ref_ptr<::osg::Material> newMaterial = new ::osg::Material(*material); @@ -983,6 +910,7 @@ void MeshShapeGeometry::extractData(bool firstTime) ss->setAttributeAndModes(depth, ::osg::StateAttribute::ON); } } else { + isColored = true; ss->setAttributeAndModes(material); // Set alpha specific properties diff --git a/dart/gui/render/MeshShapeNode.hpp b/dart/gui/render/MeshShapeNode.hpp index f4e6575a02ad1..044c7c65babb8 100644 --- a/dart/gui/render/MeshShapeNode.hpp +++ b/dart/gui/render/MeshShapeNode.hpp @@ -80,6 +80,7 @@ class DART_GUI_API MeshShapeNode : public ShapeNode, std::vector<::osg::ref_ptr<::osg::Material>> mMaterials; std::vector> mTextureImageArrays; std::vector mTemporaryTextureFiles; + std::size_t mMaterialVersion; }; } // namespace render diff --git a/dart/math/Mesh.hpp b/dart/math/Mesh.hpp index d833fd87e7a63..fe7eae7b49430 100644 --- a/dart/math/Mesh.hpp +++ b/dart/math/Mesh.hpp @@ -65,9 +65,15 @@ class Mesh /// Returns the vertices of the mesh. const Vertices& getVertices() const; + /// Returns the vertices of the mesh. + Vertices& getVertices(); + /// Returns the vertex normals of the mesh. const Normals& getVertexNormals() const; + /// Returns the vertex normals of the mesh. + Normals& getVertexNormals(); + /// Clears all the vertices and vertex normals. virtual void clear(); diff --git a/dart/math/TriMesh.hpp b/dart/math/TriMesh.hpp index 462dcf295c652..4e2eca07537fa 100644 --- a/dart/math/TriMesh.hpp +++ b/dart/math/TriMesh.hpp @@ -66,6 +66,33 @@ class TriMesh : public Mesh /// Sets vertices and triangles. void setTriangles(const Vertices& vertices, const Triangles& triangles); + /// Reserves space for vertices. + void reserveVertices(std::size_t n); + + /// Adds a vertex to the mesh. + void addVertex(S x, S y, S z); + + /// Adds a vertex to the mesh. + void addVertex(const Vector3& vertex); + + /// Reserves space for triangles. + void reserveTriangles(std::size_t n); + + /// Adds a triangle to the mesh. + void addTriangle(Index idx0, Index idx1, Index idx2); + + /// Adds a triangle to the mesh. + void addTriangle(const Triangle& triangle); + + /// Reserves space for vertex normals. + void reserveVertexNormals(std::size_t n); + + /// Adds a vertex normal to the mesh. + void addVertexNormal(S x, S y, S z); + + /// Adds a vertex normal to the mesh. + void addVertexNormal(const Vector3& normal); + /// Computes vertex normals. void computeVertexNormals(); diff --git a/dart/math/detail/Mesh-impl.hpp b/dart/math/detail/Mesh-impl.hpp index 68b80b41127b2..af844a8a609bf 100644 --- a/dart/math/detail/Mesh-impl.hpp +++ b/dart/math/detail/Mesh-impl.hpp @@ -66,6 +66,13 @@ const typename Mesh::Vertices& Mesh::getVertices() const return this->mVertices; } +//============================================================================== +template +typename Mesh::Vertices& Mesh::getVertices() +{ + return this->mVertices; +} + //============================================================================== template const typename Mesh::Normals& Mesh::getVertexNormals() const @@ -73,6 +80,13 @@ const typename Mesh::Normals& Mesh::getVertexNormals() const return this->mVertexNormals; } +//============================================================================== +template +typename Mesh::Normals& Mesh::getVertexNormals() +{ + return this->mVertexNormals; +} + //============================================================================== template void Mesh::clear() diff --git a/dart/math/detail/Random-impl.hpp b/dart/math/detail/Random-impl.hpp index fc5297fea5e3c..347fb73e2ef13 100644 --- a/dart/math/detail/Random-impl.hpp +++ b/dart/math/detail/Random-impl.hpp @@ -127,11 +127,16 @@ struct UniformMatrixImpl< const Eigen::MatrixBase& min, const Eigen::MatrixBase& max) { - const auto uniformFunc = [&](int i, int j) { - return Random::uniform(min(i, j), max(i, j)); - }; - return Derived::PlainObject::NullaryExpr( - min.rows(), min.cols(), uniformFunc); + const auto minEval = min.eval(); + const auto maxEval = max.eval(); + typename Derived::PlainObject result(minEval.rows(), minEval.cols()); + for (Eigen::Index i = 0; i < minEval.rows(); ++i) { + for (Eigen::Index j = 0; j < minEval.cols(); ++j) { + result(i, j) = Random::uniform( + minEval(i, j), maxEval(i, j)); + } + } + return result; } }; @@ -149,10 +154,14 @@ struct UniformMatrixImpl< const Eigen::MatrixBase& min, const Eigen::MatrixBase& max) { - const auto uniformFunc = [&](int i) { - return Random::uniform(min[i], max[i]); - }; - return Derived::PlainObject::NullaryExpr(min.size(), uniformFunc); + const auto minEval = min.eval(); + const auto maxEval = max.eval(); + typename Derived::PlainObject result(minEval.size()); + for (Eigen::Index i = 0; i < minEval.size(); ++i) { + result[i] + = Random::uniform(minEval[i], maxEval[i]); + } + return result; } }; @@ -170,10 +179,16 @@ struct UniformMatrixImpl< const Eigen::MatrixBase& min, const Eigen::MatrixBase& max) { - const auto uniformFunc = [&](int i, int j) { - return Random::uniform(min(i, j), max(i, j)); - }; - return Derived::PlainObject::NullaryExpr(uniformFunc); + const auto minEval = min.eval(); + const auto maxEval = max.eval(); + typename Derived::PlainObject result; + for (Eigen::Index i = 0; i < minEval.rows(); ++i) { + for (Eigen::Index j = 0; j < minEval.cols(); ++j) { + result(i, j) = Random::uniform( + minEval(i, j), maxEval(i, j)); + } + } + return result; } }; @@ -191,10 +206,14 @@ struct UniformMatrixImpl< const Eigen::MatrixBase& min, const Eigen::MatrixBase& max) { - const auto uniformFunc = [&](int i) { - return Random::uniform(min[i], max[i]); - }; - return Derived::PlainObject::NullaryExpr(uniformFunc); + const auto minEval = min.eval(); + const auto maxEval = max.eval(); + typename Derived::PlainObject result; + for (Eigen::Index i = 0; i < minEval.size(); ++i) { + result[i] + = Random::uniform(minEval[i], maxEval[i]); + } + return result; } }; diff --git a/dart/math/detail/TriMesh-impl.hpp b/dart/math/detail/TriMesh-impl.hpp index 31699abba33d9..8b18a405e5ef4 100644 --- a/dart/math/detail/TriMesh-impl.hpp +++ b/dart/math/detail/TriMesh-impl.hpp @@ -59,6 +59,69 @@ void TriMesh::setTriangles( mTriangles = triangles; } +//============================================================================== +template +void TriMesh::reserveVertices(std::size_t n) +{ + this->mVertices.reserve(n); +} + +//============================================================================== +template +void TriMesh::addVertex(S x, S y, S z) +{ + this->mVertices.emplace_back(x, y, z); +} + +//============================================================================== +template +void TriMesh::addVertex(const Vector3& vertex) +{ + this->mVertices.push_back(vertex); +} + +//============================================================================== +template +void TriMesh::reserveTriangles(std::size_t n) +{ + mTriangles.reserve(n); +} + +//============================================================================== +template +void TriMesh::addTriangle(Index idx0, Index idx1, Index idx2) +{ + mTriangles.emplace_back(idx0, idx1, idx2); +} + +//============================================================================== +template +void TriMesh::addTriangle(const Triangle& triangle) +{ + mTriangles.push_back(triangle); +} + +//============================================================================== +template +void TriMesh::reserveVertexNormals(std::size_t n) +{ + this->mVertexNormals.reserve(n); +} + +//============================================================================== +template +void TriMesh::addVertexNormal(S x, S y, S z) +{ + this->mVertexNormals.emplace_back(x, y, z); +} + +//============================================================================== +template +void TriMesh::addVertexNormal(const Vector3& normal) +{ + this->mVertexNormals.push_back(normal); +} + //============================================================================== template void TriMesh::computeVertexNormals() diff --git a/dart/utils/AssimpMeshLoader.hpp b/dart/utils/AssimpMeshLoader.hpp new file mode 100644 index 0000000000000..d5673337f602e --- /dev/null +++ b/dart/utils/AssimpMeshLoader.hpp @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_UTILS_ASSIMPMESHLOADER_HPP_ +#define DART_UTILS_ASSIMPMESHLOADER_HPP_ + +// Backward-compatibility header for the Assimp-backed mesh loader. +#include + +namespace dart { +namespace utils { + +template +using AssimpMeshLoader = MeshLoader; + +using AssimpMeshLoaderf = AssimpMeshLoader; +using AssimpMeshLoaderd = AssimpMeshLoader; + +} // namespace utils +} // namespace dart + +#endif // DART_UTILS_ASSIMPMESHLOADER_HPP_ diff --git a/dart/utils/MeshLoader.hpp b/dart/utils/MeshLoader.hpp new file mode 100644 index 0000000000000..f51e7bd4bc74e --- /dev/null +++ b/dart/utils/MeshLoader.hpp @@ -0,0 +1,348 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_UTILS_MESHLOADER_HPP_ +#define DART_UTILS_MESHLOADER_HPP_ + +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace dart { +namespace utils { + +/// Mesh loader using the Assimp library. +/// +/// This loader supports various 3D mesh formats through Assimp including: +/// - COLLADA (.dae) +/// - Wavefront OBJ (.obj) +/// - STL (.stl) +/// - PLY (.ply) +/// - And many more formats supported by Assimp +/// +/// @tparam S Scalar type for mesh coordinates (typically float or double) +template +class MeshLoader +{ +public: + using Scalar = S; + using Mesh = math::TriMesh; + + /// Constructor. + MeshLoader() = default; + + /// Destructor. + ~MeshLoader() = default; + + /// Loads a mesh from a file using Assimp. + /// + /// @param[in] filepath Path or URI to the mesh file + /// @param[in] retriever Optional resource retriever for loading from URIs + /// @return A unique pointer to the loaded mesh, or nullptr if loading fails + [[nodiscard]] std::unique_ptr load( + const std::string& filepath, + const common::ResourceRetrieverPtr& retriever = nullptr); + +private: + /// Custom deleter for aiScene that calls aiReleaseImport. + struct aiSceneDeleter + { + void operator()(const aiScene* scene) const + { + if (scene) { + aiReleaseImport(scene); + } + } + }; + + using aiScenePtr = std::unique_ptr; + + /// Loads an aiScene using Assimp with the given filepath and retriever. + static aiScenePtr loadScene( + const std::string& filepath, + const common::ResourceRetrieverPtr& retriever); +}; + +using MeshLoaderf = MeshLoader; +using MeshLoaderd = MeshLoader; + +} // namespace utils +} // namespace dart + +//============================================================================== +// +// Implementation +// +//============================================================================== + +#include + +#include + +namespace dart { +namespace utils { + +//============================================================================== +template +std::unique_ptr::Mesh> MeshLoader::load( + const std::string& filepath, const common::ResourceRetrieverPtr& retriever) +{ + // Load the scene and return nullptr if it fails + aiScenePtr scene = loadScene(filepath, retriever); + if (!scene) { + DART_WARN("[MeshLoader::load] Failed to load mesh from: {}", filepath); + return nullptr; + } + + // Return nullptr if there are no meshes + if (scene->mNumMeshes == 0) { + DART_WARN("[MeshLoader::load] No meshes found in: {}", filepath); + return nullptr; + } + + // Create the output mesh + auto mesh = std::make_unique(); + + // Merge all aiMeshes into a single TriMesh. + // This matches MeshShape behavior and ensures meshes with multiple materials + // (and therefore multiple aiMesh entries) behave as expected. + + std::size_t totalVertices = 0; + std::size_t totalFaces = 0; + for (std::size_t i = 0; i < scene->mNumMeshes; ++i) { + const aiMesh* assimpMesh = scene->mMeshes[i]; + if (!assimpMesh) { + continue; + } + totalVertices += assimpMesh->mNumVertices; + totalFaces += assimpMesh->mNumFaces; + } + + mesh->reserveVertices(totalVertices); + mesh->reserveTriangles(totalFaces); + mesh->reserveVertexNormals(totalVertices); + + for (std::size_t meshIndex = 0; meshIndex < scene->mNumMeshes; ++meshIndex) { + const aiMesh* assimpMesh = scene->mMeshes[meshIndex]; + if (!assimpMesh) { + continue; + } + + const std::size_t vertexOffset = mesh->getVertices().size(); + + // Parse vertices + for (auto i = 0u; i < assimpMesh->mNumVertices; ++i) { + const aiVector3D& vertex = assimpMesh->mVertices[i]; + mesh->addVertex( + static_cast(vertex.x), + static_cast(vertex.y), + static_cast(vertex.z)); + } + + // Parse faces (triangles) + for (auto i = 0u; i < assimpMesh->mNumFaces; ++i) { + const aiFace& face = assimpMesh->mFaces[i]; + if (face.mNumIndices != 3) { + DART_WARN( + "[MeshLoader::load] Non-triangular face detected in: {} (mesh {}). " + "Skipping this face.", + filepath, + meshIndex); + continue; + } + + mesh->addTriangle( + face.mIndices[0] + vertexOffset, + face.mIndices[1] + vertexOffset, + face.mIndices[2] + vertexOffset); + } + + // Parse vertex normals + if (assimpMesh->mNormals) { + for (auto i = 0u; i < assimpMesh->mNumVertices; ++i) { + const aiVector3D& normal = assimpMesh->mNormals[i]; + mesh->addVertexNormal( + static_cast(normal.x), + static_cast(normal.y), + static_cast(normal.z)); + } + } + } + + // Compute vertex normals if they are missing or incomplete. + if (mesh->hasTriangles() + && mesh->getVertexNormals().size() != mesh->getVertices().size()) { + mesh->computeVertexNormals(); + } + + return mesh; +} + +//============================================================================== +template +typename MeshLoader::aiScenePtr MeshLoader::loadScene( + const std::string& filepath, const common::ResourceRetrieverPtr& retriever) +{ + auto hasColladaExtension = [](const std::string& path) -> bool { + const std::size_t extensionIndex = path.find_last_of('.'); + if (extensionIndex == std::string::npos) + return false; + + std::string extension = path.substr(extensionIndex); + std::transform( + extension.begin(), extension.end(), extension.begin(), ::tolower); + return extension == ".dae" || extension == ".zae"; + }; + + auto isColladaResource = [&](const std::string& uri) -> bool { + if (hasColladaExtension(uri)) + return true; + + const auto parsedUri = common::Uri::createFromStringOrPath(uri); + if (parsedUri.mScheme.get_value_or("file") == "file" && parsedUri.mPath) { + if (hasColladaExtension(parsedUri.mPath.get())) + return true; + } + + if (!retriever) + return false; + + const auto resource = retriever->retrieve(parsedUri); + if (!resource) + return false; + + constexpr std::size_t kMaxProbeSize = 4096; + const auto sampleSize = std::min(kMaxProbeSize, resource->getSize()); + std::string buffer(sampleSize, '\0'); + const auto read = resource->read(buffer.data(), 1, sampleSize); + buffer.resize(read); + resource->seek(0, common::Resource::SEEKTYPE_SET); + + const auto upper = buffer.find("COLLADA"); + const auto lower = buffer.find("collada"); + const auto mixed = buffer.find("Collada"); + return upper != std::string::npos || lower != std::string::npos + || mixed != std::string::npos; + }; + + const bool isCollada = isColladaResource(filepath); + + // Remove points and lines from the import + aiPropertyStore* propertyStore = aiCreatePropertyStore(); + aiSetImportPropertyInteger( + propertyStore, + AI_CONFIG_PP_SBP_REMOVE, + aiPrimitiveType_POINT | aiPrimitiveType_LINE); + +#ifdef AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION + if (isCollada) { + // Keep authoring up-axis and allow us to preserve the Collada unit scale. + aiSetImportPropertyInteger( + propertyStore, AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1); + } +#endif + + // Set up the Assimp IOSystem for resource retrieval + // Suppress deprecation warnings - internal implementation detail + DART_SUPPRESS_DEPRECATED_BEGIN + std::unique_ptr systemIO; + aiFileIO fileIO; + + if (retriever) { + systemIO = std::make_unique( + retriever); + fileIO = dynamics::createFileIO(systemIO.get()); + } + DART_SUPPRESS_DEPRECATED_END + + // Import the file with post-processing flags + const aiScene* scene = aiImportFileExWithProperties( + filepath.c_str(), + aiProcess_GenNormals | aiProcess_Triangulate + | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType + | aiProcess_OptimizeMeshes, + retriever ? &fileIO : nullptr, + propertyStore); + + // If loading failed, clean up and return + if (!scene) { + DART_WARN( + "[MeshLoader::loadScene] Failed to import mesh from: {}", filepath); + DART_WARN(" Assimp error: {}", aiGetErrorString()); + aiReleasePropertyStore(propertyStore); + return nullptr; + } + +#if !defined(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION) + // Assimp rotates Collada files to align the up-axis with y-axis. + // Revert this transformation so authored coordinates remain consistent. + if (isCollada && scene->mRootNode) { + const_cast(scene)->mRootNode->mTransformation = aiMatrix4x4(); + } +#endif + + // Apply vertex pre-transformation + // We can't do this as part of the import process because we may have + // changed mTransformation above + scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices); + if (!scene) { + DART_WARN("[MeshLoader::loadScene] Failed to pre-transform vertices."); + } + + aiReleasePropertyStore(propertyStore); + + return aiScenePtr(scene); +} + +} // namespace utils +} // namespace dart + +#endif // DART_UTILS_MESHLOADER_HPP_ diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 53396f44d9664..559f2cc4e3488 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -70,6 +70,7 @@ #include "dart/math/Geometry.hpp" #include "dart/utils/CompositeResourceRetriever.hpp" #include "dart/utils/DartResourceRetriever.hpp" +#include "dart/utils/MeshLoader.hpp" #include "dart/utils/XmlHelpers.hpp" #include @@ -1379,10 +1380,14 @@ dynamics::ShapePtr readShape( const common::Uri meshUri = common::Uri::createFromRelativeUri(baseUri, filename); - const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, retriever); - if (model) { + + auto loader = std::make_unique(); + auto triMeshUnique = loader->load(meshUri.toString(), retriever); + + if (triMeshUnique) { + std::shared_ptr> triMesh(std::move(triMeshUnique)); newShape = std::make_shared( - scale, model, meshUri, retriever); + scale, std::move(triMesh), meshUri, retriever); } else { DART_ERROR("Fail to load model[{}].", filename); } diff --git a/dart/utils/mjcf/detail/Mesh.cpp b/dart/utils/mjcf/detail/Mesh.cpp index 64974cf122fb9..870f65351a6dd 100644 --- a/dart/utils/mjcf/detail/Mesh.cpp +++ b/dart/utils/mjcf/detail/Mesh.cpp @@ -33,11 +33,10 @@ #include "dart/utils/mjcf/detail/Mesh.hpp" #include "dart/dynamics/MeshShape.hpp" +#include "dart/utils/MeshLoader.hpp" #include "dart/utils/XmlHelpers.hpp" #include "dart/utils/mjcf/detail/Utils.hpp" -#include - namespace dart { namespace utils { namespace MjcfParser { @@ -114,13 +113,16 @@ Errors Mesh::postprocess(const Compiler& /*compiler*/) //============================================================================== dynamics::MeshShapePtr Mesh::createMeshShape() const { - const aiScene* model = dynamics::MeshShape::loadMesh(mMeshUri, mRetriever); - if (model == nullptr) { + auto loader = std::make_unique(); + auto triMeshUnique = loader->load(mMeshUri.toString(), mRetriever); + if (triMeshUnique == nullptr) { return nullptr; } + std::shared_ptr> triMesh(std::move(triMeshUnique)); + auto shape = std::make_shared( - mScale, model, mMeshUri, mRetriever); + mScale, std::move(triMesh), mMeshUri, mRetriever); shape->setColorMode(dynamics::MeshShape::ColorMode::MATERIAL_COLOR); return shape; } diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index e6cc7fa349055..d4999590d6836 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -56,6 +56,7 @@ #include "dart/simulation/World.hpp" #include "dart/utils/CompositeResourceRetriever.hpp" #include "dart/utils/DartResourceRetriever.hpp" +#include "dart/utils/MeshLoader.hpp" #include "dart/utils/SkelParser.hpp" #include "dart/utils/sdf/detail/GeometryParsers.hpp" #include "dart/utils/sdf/detail/SdfHelpers.hpp" @@ -303,9 +304,9 @@ dynamics::SoftBodyNode::UniqueProperties readSoftBodyProperties( const ElementPtr& softBodyNodeElement); dynamics::ShapePtr readShape( - const ElementPtr& shapelement, + const ElementPtr& _shapelement, const common::Uri& baseUri, - const common::ResourceRetrieverPtr& retriever); + const common::ResourceRetrieverPtr& _retriever); dynamics::ShapeNode* readShapeNode( dynamics::BodyNode* bodyNode, diff --git a/dart/utils/sdf/detail/GeometryParsers.cpp b/dart/utils/sdf/detail/GeometryParsers.cpp index 7f4aa32600575..8c6af2150989d 100644 --- a/dart/utils/sdf/detail/GeometryParsers.cpp +++ b/dart/utils/sdf/detail/GeometryParsers.cpp @@ -32,6 +32,7 @@ #include "dart/utils/sdf/detail/GeometryParsers.hpp" +#include "dart/common/Diagnostics.hpp" #include "dart/common/Uri.hpp" #include @@ -85,14 +86,19 @@ dynamics::ShapePtr readMeshShape( : Eigen::Vector3d::Ones(); const std::string meshUri = common::Uri::getRelativeUri(baseUri, uri); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, retriever); + DART_SUPPRESS_DEPRECATED_END if (!model) { DART_WARN("Failed to load mesh model [{}].", meshUri); return nullptr; } - return std::make_shared( - scale, model, meshUri, retriever); + DART_SUPPRESS_DEPRECATED_BEGIN + auto shape + = std::make_shared(scale, model, meshUri, retriever); + DART_SUPPRESS_DEPRECATED_END + return shape; } } // namespace diff --git a/dart/utils/urdf/UrdfParser.cpp b/dart/utils/urdf/UrdfParser.cpp index 69ff874984971..1223f3cdf4bc2 100644 --- a/dart/utils/urdf/UrdfParser.cpp +++ b/dart/utils/urdf/UrdfParser.cpp @@ -48,6 +48,7 @@ #include "dart/dynamics/WeldJoint.hpp" #include "dart/simulation/World.hpp" #include "dart/utils/DartResourceRetriever.hpp" +#include "dart/utils/MeshLoader.hpp" #include "dart/utils/urdf/BackwardCompatibility.hpp" #include "dart/utils/urdf/IncludeUrdf.hpp" #include "dart/utils/urdf/urdf_world_parser.hpp" @@ -979,14 +980,21 @@ dynamics::ShapePtr UrdfParser::createShape( // Load the mesh. const std::string resolvedUri = absoluteUri.toString(); - const aiScene* scene - = dynamics::MeshShape::loadMesh(resolvedUri, _resourceRetriever); - if (!scene) + + auto loader = std::make_unique(); + auto triMeshUnique = loader->load(resolvedUri, _resourceRetriever); + + if (!triMeshUnique) return nullptr; + std::shared_ptr> triMesh(std::move(triMeshUnique)); + const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); shape = std::make_shared( - scale, scene, resolvedUri, _resourceRetriever); + scale, + std::move(triMesh), + common::Uri(resolvedUri), + _resourceRetriever); } // Unknown geometry type else { diff --git a/docs/onboarding/README.md b/docs/onboarding/README.md index 202c5c125f0e4..b4ab19b90b874 100644 --- a/docs/onboarding/README.md +++ b/docs/onboarding/README.md @@ -103,7 +103,7 @@ DART addresses the need for: ### Quickstart (Decision Tree) -- Want to build, test, or format DART? Start with [building.md](building.md) and [contributing.md](contributing.md). +- Want to build, test, or format DART? Start with [building.md](building.md), [testing.md](testing.md), and [contributing.md](contributing.md). - Working on Gazebo / gz-physics compatibility? Jump to [build-system.md](build-system.md#gazebo-integration-feature). - Need to understand CI workflows or monitor runs? See [ci-cd.md](ci-cd.md). diff --git a/docs/onboarding/building.md b/docs/onboarding/building.md index 10b8cdb6c6a99..e40c1cea64bf8 100644 --- a/docs/onboarding/building.md +++ b/docs/onboarding/building.md @@ -2,6 +2,12 @@ This guide describes how to build DART from source, including both the C++ library and Python bindings (dartpy). +## Start here next time + +- Local test workflow: [testing.md](testing.md) +- CI monitoring and expectations: [ci-cd.md](ci-cd.md) +- Gazebo / gz-physics integration: [build-system.md](build-system.md#gazebo-integration-feature) + ## Supported Environments DART is supported on the following operating systems and compilers: @@ -150,27 +156,33 @@ We ship a [pixi](https://pixi.sh) environment for contributors. Pixi installs ev - `CMAKE_BUILD_PARALLEL_LEVEL` - controls `cmake --build` - `CTEST_PARALLEL_LEVEL` - controls `ctest` - If you want to cap parallelism, set `DART_PARALLEL_JOBS=` (Suggested (Unverified)). + If you want to cap parallelism for local runs, pick a value around two-thirds of logical cores to keep the machine responsive, then set `DART_PARALLEL_JOBS` (and `CTEST_PARALLEL_LEVEL` for ctest). Suggested (Unverified): + + ```bash + # Linux + N=$(( ( $(nproc) * 2 ) / 3 )) + # macOS + N=$(( ( $(sysctl -n hw.ncpu) * 2 ) / 3 )) + # Windows (PowerShell) + $env:N=[math]::Floor([Environment]::ProcessorCount*2/3) + ``` - Used in this task: + Example (Used in this task): ```bash pixi run lint - pixi run test - pixi run test-py - pixi run check-lint-cpp - pixi run lint-cpp - DART_PARALLEL_JOBS=16 pixi run test-all + DART_PARALLEL_JOBS=12 CTEST_PARALLEL_LEVEL=12 pixi run test + DART_PARALLEL_JOBS=12 CTEST_PARALLEL_LEVEL=12 pixi run test-all ``` Note: `pixi run test-all` runs `pixi run lint` (auto-fixing) internally; check `git status` afterwards before committing. 4. (Optional) Gazebo / gz-physics integration test: - Used in this task: + Example (Used in this task): ```bash - DART_PARALLEL_JOBS=16 pixi run -e gazebo test-gz + DART_PARALLEL_JOBS=12 CTEST_PARALLEL_LEVEL=12 pixi run -e gazebo test-gz ``` Suggested (Unverified): Run the same workflow with a different parallelism setting, e.g. `DART_PARALLEL_JOBS=8 pixi run -e gazebo test-gz`. diff --git a/docs/onboarding/ci-cd.md b/docs/onboarding/ci-cd.md index 1203fd990c7bd..86ad6a27f493a 100644 --- a/docs/onboarding/ci-cd.md +++ b/docs/onboarding/ci-cd.md @@ -14,10 +14,28 @@ DART uses GitHub Actions for continuous integration and deployment. The CI syste - Suggested (Unverified): `gh pr checks --watch --interval 30 --fail-fast` - Suggested (Unverified): `gh run view --job --log-failed` - Gotchas (observed in this task): - - On some Linux runners (especially self-hosted), `sccache` can fail during CMake `try_compile` with `sccache: error: while hashing the input file ... No such file or directory (os error 2)`. - - On macOS, `mozilla-actions/sccache-action@v0.0.9` can fail with transient network errors like `HttpError: Connect Timeout Error`. Our `.github/workflows/ci_macos.yml` treats "Setup sccache" as best-effort (`continue-on-error: true`) so the job continues without sccache. + - `gh run list` can show separate runs for `push` and `pull_request`; for PR gating, watch the `pull_request` run. + - `gh run watch` is blocking and can run for a long time; use a persistent shell and re-run it if your terminal session times out. - If `CI gz-physics` fails, reproduce locally with the Gazebo workflow in [build-system.md](build-system.md#gazebo-integration-feature). +## CI Monitoring (CLI) + +Use the GitHub CLI to locate the latest run for your branch and watch it to completion. + +Suggested (Unverified): + +```bash +gh run list -R / --branch --limit +gh run watch --interval 30 +``` + +Example (Used in this task): + +```bash +gh run list -R dartsim/dart --branch refactor/mesh_loader --limit 10 +gh run watch 20397566461 --interval 30 +``` + ## Workflow Architecture ### Core CI Workflows diff --git a/docs/onboarding/dynamics.md b/docs/onboarding/dynamics.md index 161abf1b9a4c1..0ec0b12a65bc9 100644 --- a/docs/onboarding/dynamics.md +++ b/docs/onboarding/dynamics.md @@ -520,6 +520,10 @@ const Eigen::VectorXd& C = skeleton->getCoriolisForces(); const Eigen::VectorXd& g = skeleton->getGravityForces(); ``` +## MeshShape and TriMesh + +**Design Decision:** MeshShape internally uses `dart::math::TriMesh` for mesh representation instead of Assimp's `aiScene*`. This decouples mesh data from the loading library (Assimp), enabling format-agnostic mesh handling. The deprecated `aiScene*` API is maintained for backward compatibility via lazy on-demand conversion and is scheduled for removal in DART 8.0. For OSG rendering, materials/textures are accessed through `getMaterials()` (Assimp-free), while scene graph hierarchy uses the deprecated `getMesh()` (acceptable since TriMesh doesn't include scene graph structure). See `dart/dynamics/MeshShape.hpp` and `dart/utils/MeshLoader.hpp` for implementation. + ## Summary The DART dynamics module provides a comprehensive framework for: diff --git a/docs/onboarding/python-bindings.md b/docs/onboarding/python-bindings.md index a0b054235a873..cc6a9f44b93d0 100644 --- a/docs/onboarding/python-bindings.md +++ b/docs/onboarding/python-bindings.md @@ -250,6 +250,10 @@ robot.setForces(forces) **Generation**: Can be regenerated with `pixi run generate-stubs` +## MeshShape and TriMesh Bindings + +**Design Decision:** Python bindings expose only `dart::math::TriMesh` for mesh operations, not Assimp's `aiScene*` types. This ensures Python users work with clean, format-agnostic mesh data. The MeshShape bindings provide TriMesh-based constructors and `getTriMesh()` accessor. Deprecated aiScene-based constructors were intentionally not exposed in Python bindings (breaking change allowed for dartpy). See `python/dartpy/math/TriMesh.cpp` and `python/dartpy/dynamics/Shape.cpp` for bindings implementation. + ## References - **Package configuration**: `pyproject.toml` diff --git a/docs/onboarding/testing.md b/docs/onboarding/testing.md index 4e4a05d986ae4..853a477b06bdf 100644 --- a/docs/onboarding/testing.md +++ b/docs/onboarding/testing.md @@ -2,6 +2,57 @@ This directory contains the complete test suite for DART (Dynamic Animation and Robotics Toolkit). The tests are organized by type and module to facilitate easy navigation, maintenance, and scalability. +## Start here next time + +- Local build/test workflow: [building.md](building.md) +- CI monitoring and expectations: [ci-cd.md](ci-cd.md) +- Gazebo / gz-physics integration: [build-system.md](build-system.md#gazebo-integration-feature) + +## Fast Iteration Loop + +Smallest repeatable local loop before a full CI run. + +Choose a parallelism cap around two-thirds of logical cores, then set `DART_PARALLEL_JOBS` and `CTEST_PARALLEL_LEVEL` to that value (see [building.md](building.md) for details). + +Suggested (Unverified): + +```bash +DART_PARALLEL_JOBS= CTEST_PARALLEL_LEVEL= pixi run test +``` + +Example (Used in this task): + +```bash +pixi run lint +DART_PARALLEL_JOBS=12 CTEST_PARALLEL_LEVEL=12 pixi run test +``` + +Signals to look for: + +- `ctest` ends with `100% tests passed` +- `pixi run lint` completes without modifying unrelated files + +Full validation. + +Suggested (Unverified): + +```bash +DART_PARALLEL_JOBS= CTEST_PARALLEL_LEVEL= pixi run test-all +DART_PARALLEL_JOBS= CTEST_PARALLEL_LEVEL= pixi run -e gazebo test-gz +``` + +Example (Used in this task): + +```bash +DART_PARALLEL_JOBS=12 CTEST_PARALLEL_LEVEL=12 pixi run test-all +DART_PARALLEL_JOBS=12 CTEST_PARALLEL_LEVEL=12 pixi run -e gazebo test-gz +``` + +Signals to look for: + +- `pixi run test-all` ends with `✓ All tests passed!` +- `pixi run -e gazebo test-gz` prints `✓ DART plugin built successfully with DART integration!` + ## Directory Structure ``` @@ -159,10 +210,10 @@ Performance benchmarks measure execution time and resource usage: Suggested (Unverified): If you don't know the module yet, search for similar tests by symbol name, e.g. `rg -n "" tests python`. -Examples (from this task): +Examples (Suggested, Unverified): -- `tests/integration/dynamics/test_Joints.cpp` -- `python/tests/unit/dynamics/` +- `tests/integration//test_.cpp` +- `python/tests/unit//` ### Steps to Add a New Test diff --git a/examples/box_stacking/main.cpp b/examples/box_stacking/main.cpp index 8b28689d2f283..6f08cec628523 100644 --- a/examples/box_stacking/main.cpp +++ b/examples/box_stacking/main.cpp @@ -31,6 +31,7 @@ */ #include +#include #include diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index b3e71ef832ba8..8dd3b4eeca60f 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -54,7 +55,6 @@ #include #include #include -#include #include #include @@ -540,7 +540,7 @@ class CouplerOverlay : public dart::gui::ImGuiWidget return; } - const double radToDeg = 180.0 / dart::math::constantsd::pi(); + const double radToDeg = 180.0 / dart::math::pi; const auto statuses = mController->getStatuses(); ImGui::TextWrapped( @@ -630,7 +630,7 @@ int main(int argc, char* argv[]) world->addSkeleton(couplerAssembly.skeleton); world->addSkeleton(motorAssembly.skeleton); const double targetAngle - = 45.0 * dart::math::constantsd::pi() / 180.0; // command for references + = 45.0 * dart::math::pi / 180.0; // command for references std::cout << "Coupler constraint demo:\n" diff --git a/examples/fetch/main.cpp b/examples/fetch/main.cpp index 0b4f8145cfd98..92816aadbd03b 100644 --- a/examples/fetch/main.cpp +++ b/examples/fetch/main.cpp @@ -34,6 +34,7 @@ #include #include +#include #include diff --git a/examples/heightmap/main.cpp b/examples/heightmap/main.cpp index 612ae12e59283..d89fe014306b7 100644 --- a/examples/heightmap/main.cpp +++ b/examples/heightmap/main.cpp @@ -34,9 +34,10 @@ #include #include +#include #include -#include +#include #include diff --git a/examples/imgui/main.cpp b/examples/imgui/main.cpp index 8f22ffb789109..3d3a509d422be 100644 --- a/examples/imgui/main.cpp +++ b/examples/imgui/main.cpp @@ -32,6 +32,7 @@ #include #include +#include #include diff --git a/examples/mimic_pendulums/main.cpp b/examples/mimic_pendulums/main.cpp index 981f27a43291a..119987ed6fbeb 100644 --- a/examples/mimic_pendulums/main.cpp +++ b/examples/mimic_pendulums/main.cpp @@ -46,6 +46,8 @@ #if DART_HAVE_ODE #include #endif +#include + #include #include @@ -63,7 +65,6 @@ #include #include -#include #include #include @@ -124,34 +125,6 @@ Eigen::Vector3d translationOf(const BodyNode* bn) return bn->getWorldTransform().translation(); } -std::string formatErrors(const sdf::Errors& errors) -{ - std::stringstream ss; - for (const auto& err : errors) - ss << err.Message() << "\n"; - return ss.str(); -} - -std::string readSdfText( - const dart::common::Uri& uri, - const std::shared_ptr& retriever) -{ - auto resource = retriever->retrieve(uri); - if (!resource) { - std::cerr << "Failed to retrieve SDF: " << uri.toString() << "\n"; - return {}; - } - - std::string text(resource->getSize(), '\0'); - const auto read = resource->read(text.data(), 1, text.size()); - if (read != resource->getSize()) { - std::cerr << "Failed to read SDF bytes for " << uri.toString() << "\n"; - return {}; - } - - return text; -} - struct SolverConfig { bool useOdeCollision = true; diff --git a/examples/operational_space_control/main.cpp b/examples/operational_space_control/main.cpp index 9adcf6da9aa27..e6450550d29c4 100644 --- a/examples/operational_space_control/main.cpp +++ b/examples/operational_space_control/main.cpp @@ -33,7 +33,7 @@ #include #include -#include +#include #include #include diff --git a/examples/point_cloud/main.cpp b/examples/point_cloud/main.cpp index 085e8471f50e4..3e66d65b1d9c7 100644 --- a/examples/point_cloud/main.cpp +++ b/examples/point_cloud/main.cpp @@ -34,9 +34,10 @@ #include #include +#include #include -#include +#include #include #include @@ -206,24 +207,17 @@ class PointCloudWorld : public gui::RealTimeWorldNode continue; auto mesh = std::static_pointer_cast(shape); - auto assimpScene = mesh->getMesh(); - DART_ASSERT(assimpScene); + const auto triMesh = mesh->getTriMesh(); + DART_ASSERT(triMesh); - if (assimpScene->mNumMeshes < 1) + const auto& vertices = triMesh->getVertices(); + if (vertices.empty()) continue; - const auto meshIndex - = math::Random::uniform(0, assimpScene->mNumMeshes - 1); - - auto assimpMesh = assimpScene->mMeshes[meshIndex]; - auto numVertices = assimpMesh->mNumVertices; - - auto vertexIndex - = math::Random::uniform(0, numVertices - 1); - auto vertex = assimpMesh->mVertices[vertexIndex]; Eigen::Isometry3d tf = shapeNode->getWorldTransform(); - Eigen::Vector3d eigenVertex - = Eigen::Vector3f(vertex.x, vertex.y, vertex.z).cast(); + const auto vertexIndex + = math::Random::uniform(0, vertices.size() - 1); + Eigen::Vector3d eigenVertex = vertices[vertexIndex]; eigenVertex = tf * eigenVertex; pointCloud.push_back( diff --git a/examples/simulation_event_handler/SimulationEventHandler.cpp b/examples/simulation_event_handler/SimulationEventHandler.cpp index 28837bbd163ff..8e78633c16156 100644 --- a/examples/simulation_event_handler/SimulationEventHandler.cpp +++ b/examples/simulation_event_handler/SimulationEventHandler.cpp @@ -75,7 +75,7 @@ SimulationEventHandler::SimulationEventHandler( } bool SimulationEventHandler::handle( - const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa) + const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& /*aa*/) { if (ea.getEventType() != osgGA::GUIEventAdapter::KEYDOWN) { return false; diff --git a/examples/tinkertoy/TinkertoyWidget.cpp b/examples/tinkertoy/TinkertoyWidget.cpp index 1abde54b1110b..b35f33520e4f1 100644 --- a/examples/tinkertoy/TinkertoyWidget.cpp +++ b/examples/tinkertoy/TinkertoyWidget.cpp @@ -39,6 +39,7 @@ #include "TinkertoyWidget.hpp" #include "TinkertoyWorldNode.hpp" +#include "dart/gui/IncludeImGui.hpp" //============================================================================== TinkertoyWidget::TinkertoyWidget( diff --git a/examples/wam_ikfast/Helpers.cpp b/examples/wam_ikfast/Helpers.cpp index 93a58af6c81dd..67b5aa222a3ed 100644 --- a/examples/wam_ikfast/Helpers.cpp +++ b/examples/wam_ikfast/Helpers.cpp @@ -34,7 +34,7 @@ #include -#include +#include #include diff --git a/examples/wam_ikfast/WamWorld.hpp b/examples/wam_ikfast/WamWorld.hpp index 8256f85754b7d..996fea57a0686 100644 --- a/examples/wam_ikfast/WamWorld.hpp +++ b/examples/wam_ikfast/WamWorld.hpp @@ -33,7 +33,7 @@ #include #include -#include +#include #include diff --git a/pixi.toml b/pixi.toml index d8c535f655d23..562648be151d5 100644 --- a/pixi.toml +++ b/pixi.toml @@ -82,12 +82,24 @@ DART_BUILD_TESTS_VALUE=${DART_BUILD_TESTS_OVERRIDE:-ON} DART_BUILD_GUI_RAYLIB_VALUE=${DART_BUILD_GUI_RAYLIB_OVERRIDE:-${DART_BUILD_RAYLIB_OVERRIDE:-OFF}} DART_USE_SYSTEM_RAYLIB_VALUE=${DART_USE_SYSTEM_RAYLIB_OVERRIDE:-OFF} DART_RAYLIB_GIT_TAG_VALUE=${DART_RAYLIB_GIT_TAG_OVERRIDE:-} +DART_DISABLE_COMPILER_CACHE_VALUE=${DART_DISABLE_COMPILER_CACHE:-OFF} RAYLIB_GIT_TAG_DEF="" if [ -n "${DART_RAYLIB_GIT_TAG_VALUE}" ]; then RAYLIB_GIT_TAG_DEF="-DDART_RAYLIB_GIT_TAG=${DART_RAYLIB_GIT_TAG_VALUE}" fi +if [ "${SCCACHE_GHA_ENABLED:-}" = "false" ]; then + DART_DISABLE_COMPILER_CACHE_VALUE=ON + unset DART_COMPILER_CACHE + if [ "${CMAKE_C_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_C_COMPILER_LAUNCHER + fi + if [ "${CMAKE_CXX_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_CXX_COMPILER_LAUNCHER + fi +fi if [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then - if command -v sccache >/dev/null 2>&1; then + if [ "${SCCACHE_GHA_ENABLED:-}" != "false" ] \ + && command -v sccache >/dev/null 2>&1; then CMAKE_C_COMPILER_LAUNCHER=sccache CMAKE_CXX_COMPILER_LAUNCHER=sccache elif command -v ccache >/dev/null 2>&1; then @@ -118,6 +130,7 @@ cmake \ -DDART_BUILD_EXAMPLES=${DART_BUILD_EXAMPLES_VALUE} \ -DDART_BUILD_TUTORIALS=${DART_BUILD_TUTORIALS_VALUE} \ -DDART_BUILD_TESTS=${DART_BUILD_TESTS_VALUE} \ + -DDART_DISABLE_COMPILER_CACHE=${DART_DISABLE_COMPILER_CACHE_VALUE} \ -DDART_BUILD_PROFILE=${DART_BUILD_PROFILE} \ -DDART_PROFILE_BUILTIN=${DART_PROFILE_BUILTIN} \ -DDART_PROFILE_TRACY=${DART_PROFILE_TRACY} \ @@ -267,12 +280,24 @@ DART_BUILD_TESTS_VALUE=${DART_BUILD_TESTS_OVERRIDE:-ON} DART_BUILD_GUI_RAYLIB_VALUE=${DART_BUILD_GUI_RAYLIB_OVERRIDE:-${DART_BUILD_RAYLIB_OVERRIDE:-OFF}} DART_USE_SYSTEM_RAYLIB_VALUE=${DART_USE_SYSTEM_RAYLIB_OVERRIDE:-OFF} DART_RAYLIB_GIT_TAG_VALUE=${DART_RAYLIB_GIT_TAG_OVERRIDE:-} +DART_DISABLE_COMPILER_CACHE_VALUE=${DART_DISABLE_COMPILER_CACHE:-OFF} RAYLIB_GIT_TAG_DEF="" if [ -n "${DART_RAYLIB_GIT_TAG_VALUE}" ]; then RAYLIB_GIT_TAG_DEF="-DDART_RAYLIB_GIT_TAG=${DART_RAYLIB_GIT_TAG_VALUE}" fi +if [ "${SCCACHE_GHA_ENABLED:-}" = "false" ]; then + DART_DISABLE_COMPILER_CACHE_VALUE=ON + unset DART_COMPILER_CACHE + if [ "${CMAKE_C_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_C_COMPILER_LAUNCHER + fi + if [ "${CMAKE_CXX_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_CXX_COMPILER_LAUNCHER + fi +fi if [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then - if command -v sccache >/dev/null 2>&1; then + if [ "${SCCACHE_GHA_ENABLED:-}" != "false" ] \ + && command -v sccache >/dev/null 2>&1; then CMAKE_C_COMPILER_LAUNCHER=sccache CMAKE_CXX_COMPILER_LAUNCHER=sccache elif command -v ccache >/dev/null 2>&1; then @@ -300,6 +325,7 @@ cmake \ -DDART_BUILD_EXAMPLES=${DART_BUILD_EXAMPLES_VALUE} \ -DDART_BUILD_TUTORIALS=${DART_BUILD_TUTORIALS_VALUE} \ -DDART_BUILD_TESTS=${DART_BUILD_TESTS_VALUE} \ + -DDART_DISABLE_COMPILER_CACHE=${DART_DISABLE_COMPILER_CACHE_VALUE} \ -DDART_BUILD_PROFILE=${DART_BUILD_PROFILE} \ -DDART_PROFILE_BUILTIN=${DART_PROFILE_BUILTIN} \ -DDART_PROFILE_TRACY=${DART_PROFILE_TRACY} \ @@ -339,6 +365,24 @@ CMAKE_BUILD_DIR=build/$PIXI_ENVIRONMENT_NAME/cpp/${BUILD_TYPE} python scripts/cm { arg = "build_type", default = "Release" }, ] } +build-examples = { cmd = [ + "bash", + "-lc", + """ +set -euo pipefail +BUILD_TYPE={{ build_type }} +CMAKE_BUILD_DIR=build/$PIXI_ENVIRONMENT_NAME/cpp/${BUILD_TYPE} python scripts/cmake_build.py --target examples +""", +], depends-on = [ + { task = "config", args = [ + "{{ dartpy }}", + "{{ build_type }}", + ] }, +], args = [ + { arg = "dartpy", default = "ON" }, + { arg = "build_type", default = "Release" }, +] } + build-dart8-tests = { cmd = [ "bash", "-lc", @@ -393,7 +437,7 @@ CMAKE_BUILD_DIR=build/$PIXI_ENVIRONMENT_NAME/cpp/${BUILD_TYPE} python scripts/cm config-py = { cmd = [ "bash", "-lc", - "set -euo pipefail\nBUILD_TYPE={{ build_type }}\nDART_BUILD_PROFILE=${DART_BUILD_PROFILE:-ON}\nDART_PROFILE_BUILTIN=${DART_PROFILE_BUILTIN:-ON}\nDART_PROFILE_TRACY=${DART_PROFILE_TRACY:-ON}\nDART_ENABLE_ASAN=${DART_ENABLE_ASAN:-OFF}\nDART_VERBOSE=${DART_VERBOSE:-OFF}\nDART_BUILD_DARTPY_VALUE=${DART_BUILD_DARTPY_OVERRIDE:-{{ dartpy }}}\nif [ -z \"${DART_BUILD_DARTPY_VALUE}\" ]; then\n DART_BUILD_DARTPY_VALUE=\"{{ dartpy }}\"\nfi\nif [ -z \"${CMAKE_C_COMPILER_LAUNCHER:-}\" ]; then\n if command -v sccache >/dev/null 2>&1; then\n CMAKE_C_COMPILER_LAUNCHER=sccache\n CMAKE_CXX_COMPILER_LAUNCHER=sccache\n elif command -v ccache >/dev/null 2>&1; then\n CMAKE_C_COMPILER_LAUNCHER=ccache\n CMAKE_CXX_COMPILER_LAUNCHER=ccache\n fi\nfi\nLAUNCHER_DEFS=\"\"\nif [ -n \"${CMAKE_C_COMPILER_LAUNCHER:-}\" ]; then\n LAUNCHER_DEFS=\"$LAUNCHER_DEFS -DCMAKE_C_COMPILER_LAUNCHER=${CMAKE_C_COMPILER_LAUNCHER}\"\nfi\nif [ -n \"${CMAKE_CXX_COMPILER_LAUNCHER:-}\" ]; then\n LAUNCHER_DEFS=\"$LAUNCHER_DEFS -DCMAKE_CXX_COMPILER_LAUNCHER=${CMAKE_CXX_COMPILER_LAUNCHER}\"\nfi\ncmake -G Ninja -S . -B build/$PIXI_ENVIRONMENT_NAME/cpp/${BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCMAKE_PREFIX_PATH=$CONDA_PREFIX -DDART_BUILD_DARTPY=${DART_BUILD_DARTPY_VALUE} -DDART_BUILD_DART8=ON -DDART_BUILD_PROFILE=${DART_BUILD_PROFILE} -DDART_PROFILE_BUILTIN=${DART_PROFILE_BUILTIN} -DDART_PROFILE_TRACY=${DART_PROFILE_TRACY} -DDART_ENABLE_ASAN=${DART_ENABLE_ASAN} -DDART_USE_SYSTEM_GOOGLEBENCHMARK=ON -DDART_USE_SYSTEM_GOOGLETEST=ON -DDART_USE_SYSTEM_IMGUI=ON -DDART_USE_SYSTEM_NANOBIND=OFF -DDART_USE_SYSTEM_TRACY=ON -DDART_VERBOSE=${DART_VERBOSE} $LAUNCHER_DEFS", + "set -euo pipefail\nBUILD_TYPE={{ build_type }}\nDART_BUILD_PROFILE=${DART_BUILD_PROFILE:-ON}\nDART_PROFILE_BUILTIN=${DART_PROFILE_BUILTIN:-ON}\nDART_PROFILE_TRACY=${DART_PROFILE_TRACY:-ON}\nDART_ENABLE_ASAN=${DART_ENABLE_ASAN:-OFF}\nDART_VERBOSE=${DART_VERBOSE:-OFF}\nDART_BUILD_DARTPY_VALUE=${DART_BUILD_DARTPY_OVERRIDE:-{{ dartpy }}}\nif [ -z \"${DART_BUILD_DARTPY_VALUE}\" ]; then\n DART_BUILD_DARTPY_VALUE=\"{{ dartpy }}\"\nfi\nDART_DISABLE_COMPILER_CACHE_VALUE=${DART_DISABLE_COMPILER_CACHE:-OFF}\nif [ \"${SCCACHE_GHA_ENABLED:-}\" = \"false\" ]; then\n DART_DISABLE_COMPILER_CACHE_VALUE=ON\n unset DART_COMPILER_CACHE\n if [ \"${CMAKE_C_COMPILER_LAUNCHER:-}\" = \"sccache\" ]; then\n unset CMAKE_C_COMPILER_LAUNCHER\n fi\n if [ \"${CMAKE_CXX_COMPILER_LAUNCHER:-}\" = \"sccache\" ]; then\n unset CMAKE_CXX_COMPILER_LAUNCHER\n fi\nfi\nif [ -z \"${CMAKE_C_COMPILER_LAUNCHER:-}\" ]; then\n if [ \"${SCCACHE_GHA_ENABLED:-}\" != \"false\" ] && command -v sccache >/dev/null 2>&1; then\n CMAKE_C_COMPILER_LAUNCHER=sccache\n CMAKE_CXX_COMPILER_LAUNCHER=sccache\n elif command -v ccache >/dev/null 2>&1; then\n CMAKE_C_COMPILER_LAUNCHER=ccache\n CMAKE_CXX_COMPILER_LAUNCHER=ccache\n fi\nfi\nLAUNCHER_DEFS=\"\"\nif [ -n \"${CMAKE_C_COMPILER_LAUNCHER:-}\" ]; then\n LAUNCHER_DEFS=\"$LAUNCHER_DEFS -DCMAKE_C_COMPILER_LAUNCHER=${CMAKE_C_COMPILER_LAUNCHER}\"\nfi\nif [ -n \"${CMAKE_CXX_COMPILER_LAUNCHER:-}\" ]; then\n LAUNCHER_DEFS=\"$LAUNCHER_DEFS -DCMAKE_CXX_COMPILER_LAUNCHER=${CMAKE_CXX_COMPILER_LAUNCHER}\"\nfi\ncmake -G Ninja -S . -B build/$PIXI_ENVIRONMENT_NAME/cpp/${BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCMAKE_PREFIX_PATH=$CONDA_PREFIX -DDART_BUILD_DARTPY=${DART_BUILD_DARTPY_VALUE} -DDART_BUILD_DART8=ON -DDART_DISABLE_COMPILER_CACHE=${DART_DISABLE_COMPILER_CACHE_VALUE} -DDART_BUILD_PROFILE=${DART_BUILD_PROFILE} -DDART_PROFILE_BUILTIN=${DART_PROFILE_BUILTIN} -DDART_PROFILE_TRACY=${DART_PROFILE_TRACY} -DDART_ENABLE_ASAN=${DART_ENABLE_ASAN} -DDART_USE_SYSTEM_GOOGLEBENCHMARK=ON -DDART_USE_SYSTEM_GOOGLETEST=ON -DDART_USE_SYSTEM_IMGUI=ON -DDART_USE_SYSTEM_NANOBIND=OFF -DDART_USE_SYSTEM_TRACY=ON -DDART_VERBOSE=${DART_VERBOSE} $LAUNCHER_DEFS", ], env = { DART_VERBOSE = "OFF" }, args = [ { arg = "dartpy", default = "ON" }, { arg = "build_type", default = "Release" }, @@ -615,8 +659,20 @@ set -euo pipefail PIXI_ENVIRONMENT_NAME=${PIXI_ENVIRONMENT_NAME:-default} rm -rf "build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE" echo "Configuring coverage build directory: build/$PIXI_ENVIRONMENT_NAME/cpp/$BUILD_TYPE" +DART_DISABLE_COMPILER_CACHE_VALUE=${DART_DISABLE_COMPILER_CACHE:-OFF} +if [ "${SCCACHE_GHA_ENABLED:-}" = "false" ]; then + DART_DISABLE_COMPILER_CACHE_VALUE=ON + unset DART_COMPILER_CACHE + if [ "${CMAKE_C_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_C_COMPILER_LAUNCHER + fi + if [ "${CMAKE_CXX_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_CXX_COMPILER_LAUNCHER + fi +fi if [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then - if command -v sccache >/dev/null 2>&1; then + if [ "${SCCACHE_GHA_ENABLED:-}" != "false" ] \ + && command -v sccache >/dev/null 2>&1; then CMAKE_C_COMPILER_LAUNCHER=sccache CMAKE_CXX_COMPILER_LAUNCHER=sccache elif command -v ccache >/dev/null 2>&1; then @@ -639,6 +695,7 @@ cmake \ -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ -DCMAKE_PREFIX_PATH=$CONDA_PREFIX \ -DDART_BUILD_PROFILE=OFF \ + -DDART_DISABLE_COMPILER_CACHE=${DART_DISABLE_COMPILER_CACHE_VALUE} \ -DDART_CODECOV=ON \ -DDART_USE_SYSTEM_GOOGLEBENCHMARK=ON \ -DDART_USE_SYSTEM_GOOGLETEST=ON \ @@ -867,6 +924,34 @@ config-install = { cmd = [ "-lc", """ set -euo pipefail +DART_DISABLE_COMPILER_CACHE_VALUE=${DART_DISABLE_COMPILER_CACHE:-OFF} +if [ "${SCCACHE_GHA_ENABLED:-}" = "false" ]; then + DART_DISABLE_COMPILER_CACHE_VALUE=ON + unset DART_COMPILER_CACHE + if [ "${CMAKE_C_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_C_COMPILER_LAUNCHER + fi + if [ "${CMAKE_CXX_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_CXX_COMPILER_LAUNCHER + fi +fi +if [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then + if [ "${SCCACHE_GHA_ENABLED:-}" != "false" ] \ + && command -v sccache >/dev/null 2>&1; then + CMAKE_C_COMPILER_LAUNCHER=sccache + CMAKE_CXX_COMPILER_LAUNCHER=sccache + elif command -v ccache >/dev/null 2>&1; then + CMAKE_C_COMPILER_LAUNCHER=ccache + CMAKE_CXX_COMPILER_LAUNCHER=ccache + fi +fi +LAUNCHER_DEFS="" +if [ -n "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then + LAUNCHER_DEFS="$LAUNCHER_DEFS -DCMAKE_C_COMPILER_LAUNCHER=${CMAKE_C_COMPILER_LAUNCHER}" +fi +if [ -n "${CMAKE_CXX_COMPILER_LAUNCHER:-}" ]; then + LAUNCHER_DEFS="$LAUNCHER_DEFS -DCMAKE_CXX_COMPILER_LAUNCHER=${CMAKE_CXX_COMPILER_LAUNCHER}" +fi cmake \ -G Ninja \ -S . \ @@ -879,13 +964,15 @@ cmake \ -DDART_BUILD_GUI=${DART_BUILD_GUI_OVERRIDE:-ON} \ -DDART_BUILD_COLLISION_BULLET=${DART_BUILD_COLLISION_BULLET_OVERRIDE:-ON} \ -DDART_BUILD_COLLISION_ODE=${DART_BUILD_COLLISION_ODE_OVERRIDE:-ON} \ + -DDART_DISABLE_COMPILER_CACHE=${DART_DISABLE_COMPILER_CACHE_VALUE} \ -DDART_BUILD_PROFILE=OFF \ -DDART_USE_SYSTEM_GOOGLEBENCHMARK=ON \ -DDART_USE_SYSTEM_GOOGLETEST=ON \ -DDART_USE_SYSTEM_IMGUI=ON \ -DDART_USE_SYSTEM_NANOBIND=OFF \ -DDART_USE_SYSTEM_TRACY=ON \ - -DDART_VERBOSE=$DART_VERBOSE + -DDART_VERBOSE=$DART_VERBOSE \ + $LAUNCHER_DEFS """, ], env = { DART_VERBOSE = "OFF", BUILD_TYPE = "Release" }, args = [ { arg = "dartpy", default = "ON" }, @@ -945,9 +1032,9 @@ verify-wheel = { cmd = "bash -c 'python scripts/verify_wheel.py dist/dartpy-*.wh test-installation = { cmd = "python scripts/test_installation.py" } # Shared wheel helper tasks (reused by version-specific wheel features) -wheel-build-core = { cmd = "pip wheel . --no-deps -w dist/ -v", args = [ +wheel-build-core = { cmd = "python scripts/wheel_build.py {{ use_system_imgui }}", args = [ { arg = "use_system_imgui", default = "ON" }, -], env = { CMAKE_ARGS = "-DCMAKE_PREFIX_PATH=$CONDA_PREFIX -DDART_BUILD_DARTPY=ON -DDART_BUILD_GUI=ON -DDART_ENABLE_SIMD=OFF -DDART_BUILD_WHEELS=ON -DDART_TREAT_WARNINGS_AS_ERRORS=OFF -DDART_BUILD_TESTS=OFF -DDART_BUILD_EXAMPLES=OFF -DDART_BUILD_TUTORIALS=OFF -DBUILD_SHARED_LIBS=OFF -DDART_USE_SYSTEM_IMGUI={{ use_system_imgui }}" } } +] } wheel-repair-linux-core = { cmd = "auditwheel repair dist/dartpy-*-{{ python_tag }}-*-linux_x86_64.whl -w dist/ && rm dist/dartpy-*-{{ python_tag }}-*-linux_x86_64.whl", args = [ { arg = "python_tag" }, @@ -1065,7 +1152,20 @@ DART_BUILD_PROFILE=${DART_BUILD_PROFILE:-OFF} DART_VERBOSE=${DART_VERBOSE:-OFF} DART_BUILD_DARTPY_VALUE=${DART_BUILD_DARTPY_OVERRIDE:-ON} DART_BUILD_DART8_VALUE=${DART_BUILD_DART8_OVERRIDE:-ON} -if command -v sccache >/dev/null 2>&1 && [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then +DART_DISABLE_COMPILER_CACHE_VALUE=${DART_DISABLE_COMPILER_CACHE:-OFF} +if [ "${SCCACHE_GHA_ENABLED:-}" = "false" ]; then + DART_DISABLE_COMPILER_CACHE_VALUE=ON + unset DART_COMPILER_CACHE + if [ "${CMAKE_C_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_C_COMPILER_LAUNCHER + fi + if [ "${CMAKE_CXX_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_CXX_COMPILER_LAUNCHER + fi +fi +if [ "${SCCACHE_GHA_ENABLED:-}" != "false" ] \ + && command -v sccache >/dev/null 2>&1 \ + && [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then CMAKE_C_COMPILER_LAUNCHER=sccache CMAKE_CXX_COMPILER_LAUNCHER=sccache fi @@ -1085,6 +1185,7 @@ cmake \ -DCMAKE_PREFIX_PATH=$CONDA_PREFIX \ -DDART_BUILD_DARTPY=${DART_BUILD_DARTPY_VALUE} \ -DDART_BUILD_DART8=${DART_BUILD_DART8_VALUE} \ + -DDART_DISABLE_COMPILER_CACHE=${DART_DISABLE_COMPILER_CACHE_VALUE} \ -DDART_BUILD_PROFILE=${DART_BUILD_PROFILE} \ -DDART_ENABLE_ASAN=ON \ -DDART_USE_SYSTEM_GOOGLEBENCHMARK=ON \ @@ -1611,7 +1712,17 @@ config-gz = { cmd = [ "-lc", """ set -euo pipefail -if command -v sccache >/dev/null 2>&1 && [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then +if [ "${SCCACHE_GHA_ENABLED:-}" = "false" ]; then + if [ "${CMAKE_C_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_C_COMPILER_LAUNCHER + fi + if [ "${CMAKE_CXX_COMPILER_LAUNCHER:-}" = "sccache" ]; then + unset CMAKE_CXX_COMPILER_LAUNCHER + fi +fi +if [ "${SCCACHE_GHA_ENABLED:-}" != "false" ] \ + && command -v sccache >/dev/null 2>&1 \ + && [ -z "${CMAKE_C_COMPILER_LAUNCHER:-}" ]; then CMAKE_C_COMPILER_LAUNCHER=sccache CMAKE_CXX_COMPILER_LAUNCHER=sccache fi diff --git a/python/dartpy/CMakeLists.txt b/python/dartpy/CMakeLists.txt index bfe40d813a9e1..8e9e947044efa 100644 --- a/python/dartpy/CMakeLists.txt +++ b/python/dartpy/CMakeLists.txt @@ -104,6 +104,7 @@ set(dartpy_headers math/geometry.hpp math/random.hpp math/eigen_geometry.hpp + math/trimesh.hpp dynamics/module.hpp dynamics/entity.hpp dynamics/degree_of_freedom.hpp @@ -179,6 +180,7 @@ set(dartpy_sources math/geometry.cpp math/random.cpp math/eigen_geometry.cpp + math/TriMesh.cpp dynamics/module.cpp dynamics/entity.cpp dynamics/degree_of_freedom.cpp diff --git a/python/dartpy/dynamics/shape.cpp b/python/dartpy/dynamics/shape.cpp index 42c11bb8b121f..534364d0b4c0d 100644 --- a/python/dartpy/dynamics/shape.cpp +++ b/python/dartpy/dynamics/shape.cpp @@ -3,8 +3,10 @@ #include "common/eigen_utils.hpp" #include "common/repr.hpp" #include "dart/dynamics/BoxShape.hpp" +#include "dart/dynamics/MeshShape.hpp" #include "dart/dynamics/Shape.hpp" #include "dart/dynamics/SphereShape.hpp" +#include "dart/math/TriMesh.hpp" #include #include @@ -120,6 +122,36 @@ void defShape(nb::module_& m) fields.emplace_back("size", size_stream.str()); return format_repr("BoxShape", fields); }); + + nb::class_(m, "MeshShape") + .def( + nb::new_( + [](const nb::handle& scale, + const std::shared_ptr>& mesh) { + nb::sequence seq = nb::cast(scale); + if (nb::len(seq) != 3) { + throw nb::type_error("MeshShape scale must have length 3"); + } + + Eigen::Vector3d vec; + for (size_t i = 0; i < 3; ++i) { + vec[i] = nb::cast(seq[i]); + } + + return std::make_shared(vec, mesh); + }), + nb::arg("scale"), + nb::arg("mesh")) + .def("getTriMesh", &dart::dynamics::MeshShape::getTriMesh) + .def( + "getScale", + [](const dart::dynamics::MeshShape& self) { return self.getScale(); }) + .def_static( + "getStaticType", + []() -> const std::string& { + return dart::dynamics::MeshShape::getStaticType(); + }, + nb::rv_policy::reference_internal); } } // namespace dart::python_nb diff --git a/python/dartpy/math/TriMesh.cpp b/python/dartpy/math/TriMesh.cpp new file mode 100644 index 0000000000000..f570197e3cded --- /dev/null +++ b/python/dartpy/math/TriMesh.cpp @@ -0,0 +1,167 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "math/trimesh.hpp" + +#include "dart/math/TriMesh.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +namespace nb = nanobind; + +namespace dart::python_nb { + +namespace { + +Eigen::Vector3d toVec3(const nb::handle& h) +{ + try { + return nb::cast(h); + } catch (const nb::cast_error&) { + nb::sequence seq = nb::cast(h); + if (nb::len(seq) != 3) + throw nb::type_error("Expected a length-3 sequence"); + + Eigen::Vector3d vec; + for (nb::ssize_t i = 0; i < 3; ++i) + vec[i] = nb::cast(seq[i]); + return vec; + } +} + +} // namespace + +void defTriMesh(nb::module_& m) +{ + using TriMeshd = dart::math::TriMesh; + + nb::class_(m, "TriMesh") + .def( + nb::new_([]() { return std::make_shared(); }), + "Create an empty TriMesh") + .def( + "addVertex", + [](TriMeshd& self, const nb::handle& vertex) { + self.addVertex(toVec3(vertex)); + }, + nb::arg("vertex"), + "Add a vertex to the mesh") + .def( + "addTriangle", + [](TriMeshd& self, std::size_t v0, std::size_t v1, std::size_t v2) { + self.addTriangle(v0, v1, v2); + }, + nb::arg("v0"), + nb::arg("v1"), + nb::arg("v2"), + "Add a triangle (by vertex indices)") + .def( + "addVertexNormal", + [](TriMeshd& self, const nb::handle& normal) { + self.addVertexNormal(toVec3(normal)); + }, + nb::arg("normal"), + "Add a vertex normal") + .def( + "reserveVertices", + &TriMeshd::reserveVertices, + nb::arg("count"), + "Reserve space for vertices") + .def( + "reserveTriangles", + &TriMeshd::reserveTriangles, + nb::arg("count"), + "Reserve space for triangles") + .def( + "reserveVertexNormals", + &TriMeshd::reserveVertexNormals, + nb::arg("count"), + "Reserve space for vertex normals") + .def( + "getVertices", + [](const TriMeshd& self) { + return std::vector( + self.getVertices().begin(), self.getVertices().end()); + }, + "Get the list of vertices") + .def( + "getTriangles", + [](const TriMeshd& self) { + std::vector triangles; + triangles.reserve(self.getTriangles().size()); + for (const auto& triangle : self.getTriangles()) { + triangles.emplace_back( + static_cast(triangle[0]), + static_cast(triangle[1]), + static_cast(triangle[2])); + } + return triangles; + }, + "Get the list of triangles (vertex indices)") + .def( + "getVertexNormals", + [](const TriMeshd& self) { + return std::vector( + self.getVertexNormals().begin(), self.getVertexNormals().end()); + }, + "Get the list of vertex normals") + .def( + "hasVertexNormals", + &TriMeshd::hasVertexNormals, + "Check if the mesh has vertex normals") + .def( + "computeVertexNormals", + &TriMeshd::computeVertexNormals, + "Compute vertex normals") + .def( + "getNumVertices", + [](const TriMeshd& self) { return self.getVertices().size(); }, + "Get the number of vertices") + .def( + "getNumTriangles", + [](const TriMeshd& self) { return self.getTriangles().size(); }, + "Get the number of triangles") + .def( + "clear", + &TriMeshd::clear, + "Clear all vertices, triangles, and normals"); +} + +} // namespace dart::python_nb diff --git a/python/dartpy/math/module.cpp b/python/dartpy/math/module.cpp index 01cfcea51d74a..847be53c1f301 100644 --- a/python/dartpy/math/module.cpp +++ b/python/dartpy/math/module.cpp @@ -36,6 +36,7 @@ #include "math/eigen_geometry.hpp" #include "math/geometry.hpp" #include "math/random.hpp" +#include "math/trimesh.hpp" namespace dart::python_nb { @@ -45,6 +46,7 @@ void defMathModule(nanobind::module_& m) defRandom(m); defGeometry(m); defEigenGeometry(m); + defTriMesh(m); } } // namespace dart::python_nb diff --git a/python/dartpy/math/trimesh.hpp b/python/dartpy/math/trimesh.hpp new file mode 100644 index 0000000000000..4b13dbbfebdcc --- /dev/null +++ b/python/dartpy/math/trimesh.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include + +namespace dart::python_nb { + +void defTriMesh(nanobind::module_& m); + +} // namespace dart::python_nb diff --git a/python/tests/unit/dynamics/test_meshshape.py b/python/tests/unit/dynamics/test_meshshape.py new file mode 100644 index 0000000000000..830649e456fd0 --- /dev/null +++ b/python/tests/unit/dynamics/test_meshshape.py @@ -0,0 +1,25 @@ +import dartpy as dart +import numpy as np +import pytest + + +def test_meshshape_trimesh_roundtrip(): + tri_mesh = dart.TriMesh() + tri_mesh.add_vertex([0.0, 0.0, 0.0]) + tri_mesh.add_vertex([1.0, 0.0, 0.0]) + tri_mesh.add_vertex([0.0, 1.0, 0.0]) + tri_mesh.add_triangle(0, 1, 2) + + scale = [1.0, 2.0, 3.0] + shape = dart.MeshShape(scale, tri_mesh) + + assert shape.get_type() == dart.MeshShape.get_static_type() + np.testing.assert_allclose(np.asarray(shape.get_scale()), np.asarray(scale)) + + retrieved = shape.get_tri_mesh() + assert retrieved.get_num_vertices() == tri_mesh.get_num_vertices() + assert retrieved.get_num_triangles() == tri_mesh.get_num_triangles() + + +if __name__ == "__main__": + pytest.main() diff --git a/python/tests/unit/math/test_trimesh.py b/python/tests/unit/math/test_trimesh.py new file mode 100644 index 0000000000000..24ca1cb59635d --- /dev/null +++ b/python/tests/unit/math/test_trimesh.py @@ -0,0 +1,74 @@ +import dartpy as dart +import numpy as np +import pytest + + +def test_trimesh_basic(): + mesh = dart.TriMesh() + mesh.reserve_vertices(3) + mesh.reserve_triangles(1) + mesh.reserve_vertex_normals(3) + + mesh.add_vertex([0.0, 0.0, 0.0]) + mesh.add_vertex([1.0, 0.0, 0.0]) + mesh.add_vertex([0.0, 1.0, 0.0]) + mesh.add_triangle(0, 1, 2) + + assert mesh.get_num_vertices() == 3 + assert mesh.get_num_triangles() == 1 + + vertices = mesh.get_vertices() + assert len(vertices) == 3 + np.testing.assert_allclose(np.asarray(vertices[1]), np.array([1.0, 0.0, 0.0])) + + assert not mesh.has_vertex_normals() + mesh.add_vertex_normal([0.0, 0.0, 1.0]) + mesh.add_vertex_normal([0.0, 0.0, 1.0]) + mesh.add_vertex_normal([0.0, 0.0, 1.0]) + assert mesh.has_vertex_normals() + + normals = mesh.get_vertex_normals() + assert len(normals) == 3 + np.testing.assert_allclose(np.asarray(normals[0]), np.array([0.0, 0.0, 1.0])) + + triangles = mesh.get_triangles() + assert len(triangles) == 1 + np.testing.assert_array_equal(np.asarray(triangles[0]), np.array([0, 1, 2])) + + +def test_trimesh_compute_vertex_normals(): + mesh = dart.TriMesh() + mesh.add_vertex([0.0, 0.0, 0.0]) + mesh.add_vertex([1.0, 0.0, 0.0]) + mesh.add_vertex([0.0, 1.0, 0.0]) + mesh.add_triangle(0, 1, 2) + + assert not mesh.has_vertex_normals() + mesh.compute_vertex_normals() + assert mesh.has_vertex_normals() + + normals = mesh.get_vertex_normals() + assert len(normals) == 3 + expected = np.array([0.0, 0.0, 1.0]) + for normal in normals: + np.testing.assert_allclose(np.asarray(normal), expected) + + +def test_trimesh_clear(): + mesh = dart.TriMesh() + mesh.add_vertex([0.0, 0.0, 0.0]) + mesh.add_vertex([1.0, 0.0, 0.0]) + mesh.add_vertex([0.0, 1.0, 0.0]) + mesh.add_triangle(0, 1, 2) + + assert mesh.get_num_vertices() == 3 + assert mesh.get_num_triangles() == 1 + + mesh.clear() + assert mesh.get_num_vertices() == 0 + assert mesh.get_num_triangles() == 0 + assert not mesh.has_vertex_normals() + + +if __name__ == "__main__": + pytest.main() diff --git a/scripts/test_all.py b/scripts/test_all.py index a0c6292b78c2f..9ef9b9162cbf6 100755 --- a/scripts/test_all.py +++ b/scripts/test_all.py @@ -5,6 +5,7 @@ This script runs all available tests locally: - Linting (C++ and Python) - Building (Release and Debug) +- Building (tests and examples) - Unit tests - Python tests - Documentation build @@ -359,6 +360,21 @@ def run_build_tests(skip_debug: bool = False) -> bool: result, _ = run_command(pixi_command("build"), "Build Release") success = success and result + if result: + # Build all tests (compile-only; running happens in the Unit Tests step) + result, _ = run_command( + pixi_command("build-tests", PIXI_DEFAULT_DARTPY, "Release"), + "Build tests (Release)", + ) + success = success and result + + # Build all examples (compile-only) + result, _ = run_command( + pixi_command("build-examples", PIXI_DEFAULT_DARTPY, "Release"), + "Build examples (Release)", + ) + success = success and result + if not skip_debug: # Build Debug (for better error messages) result, _ = run_command(pixi_command("build-debug"), "Build Debug") diff --git a/scripts/wheel_build.py b/scripts/wheel_build.py new file mode 100644 index 0000000000000..0259bd1e307a5 --- /dev/null +++ b/scripts/wheel_build.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import os +import shlex +import subprocess +import sys +from pathlib import Path + + +def _is_false(value: str | None) -> bool: + if value is None: + return False + return value.strip().lower() in {"0", "false", "off", "no"} + + +def _is_sccache_launcher(value: str | None) -> bool: + if not value: + return False + launcher_name = Path(value).name.lower() + return launcher_name in {"sccache", "sccache.exe"} + + +def main(argv: list[str]) -> int: + use_system_imgui = argv[1].upper() if len(argv) > 1 else "ON" + if use_system_imgui not in {"ON", "OFF"}: + print( + f"Invalid use_system_imgui value '{use_system_imgui}'. Expected ON/OFF.", + file=sys.stderr, + ) + return 2 + + conda_prefix = os.environ.get("CONDA_PREFIX") + if not conda_prefix: + print( + "CONDA_PREFIX is not set; wheel build must run inside a pixi env.", + file=sys.stderr, + ) + return 2 + + disable_compiler_cache = os.environ.get( + "DART_DISABLE_COMPILER_CACHE", "OFF" + ).upper() + if _is_false(os.environ.get("SCCACHE_GHA_ENABLED")): + disable_compiler_cache = "ON" + + if disable_compiler_cache == "ON": + os.environ.pop("DART_COMPILER_CACHE", None) + for launcher_var in ( + "CMAKE_C_COMPILER_LAUNCHER", + "CMAKE_CXX_COMPILER_LAUNCHER", + ): + if _is_sccache_launcher(os.environ.get(launcher_var)): + os.environ.pop(launcher_var, None) + + cmake_args = [ + f"-DCMAKE_PREFIX_PATH={conda_prefix}", + "-DDART_BUILD_DARTPY=ON", + "-DDART_BUILD_GUI=ON", + "-DDART_ENABLE_SIMD=OFF", + "-DDART_BUILD_WHEELS=ON", + "-DDART_TREAT_WARNINGS_AS_ERRORS=OFF", + "-DDART_BUILD_TESTS=OFF", + "-DDART_BUILD_EXAMPLES=OFF", + "-DDART_BUILD_TUTORIALS=OFF", + "-DBUILD_SHARED_LIBS=OFF", + f"-DDART_USE_SYSTEM_IMGUI={use_system_imgui}", + f"-DDART_DISABLE_COMPILER_CACHE={disable_compiler_cache}", + ] + + os.environ["DART_DISABLE_COMPILER_CACHE"] = disable_compiler_cache + os.environ["CMAKE_ARGS"] = " ".join(cmake_args) + + Path("dist").mkdir(parents=True, exist_ok=True) + + cmd = ["pip", "wheel", ".", "--no-deps", "-w", "dist/", "-v"] + print(f"Running: {' '.join(shlex.quote(part) for part in cmd)}") + print(f"CMAKE_ARGS={os.environ['CMAKE_ARGS']}") + subprocess.run(cmd, check=True) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv)) diff --git a/tests/integration/collision/test_PlaneShapeCollision.cpp b/tests/integration/collision/test_PlaneShapeCollision.cpp index a69f53b0ca01d..9597ce92d3b7b 100644 --- a/tests/integration/collision/test_PlaneShapeCollision.cpp +++ b/tests/integration/collision/test_PlaneShapeCollision.cpp @@ -48,6 +48,8 @@ #include #include +#include + #include #include @@ -99,11 +101,19 @@ void runIssue1234Test( std::function detectorFactory) { const std::string meshUri = "dart://sample/obj/BoxSmall.obj"; - const auto aiscene = dart::dynamics::MeshShape::loadMesh( - meshUri, dart::utils::DartResourceRetriever::create()); - ASSERT_TRUE(aiscene); + + // Load mesh using MeshShape::loadMesh + const auto retriever = dart::utils::DartResourceRetriever::create(); + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* aiMesh + = dart::dynamics::MeshShape::loadMesh(meshUri, retriever); + DART_SUPPRESS_DEPRECATED_END + + ASSERT_TRUE(aiMesh); + DART_SUPPRESS_DEPRECATED_BEGIN const auto mesh = std::make_shared( - 100.0 * Eigen::Vector3d::Ones(), aiscene, meshUri); + 100.0 * Eigen::Vector3d::Ones(), aiMesh, dart::common::Uri(meshUri)); + DART_SUPPRESS_DEPRECATED_END const auto bb = mesh->getBoundingBox(); for (int i = 0; i < 3; ++i) { diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index 6305d5b013eaf..46ac1f0f4abc0 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -119,6 +119,13 @@ endif() if(TARGET dart-gui) dart_add_test( "unit" UNIT_gui_ImGuiWindowScaling gui/test_ImGuiWindowScaling.cpp) + # System ImGui builds can depend on SDL3, which is not available in ASAN CI. + if(NOT (DART_ENABLE_ASAN AND DART_USE_SYSTEM_IMGUI)) + dart_add_test( + "unit" UNIT_gui_MeshShapeNodeMaterialUpdates + gui/test_MeshShapeNodeMaterialUpdates.cpp) + target_link_libraries(UNIT_gui_MeshShapeNodeMaterialUpdates dart-gui) + endif() endif() # ============================================================================== @@ -127,6 +134,9 @@ endif() dart_add_test("unit" test_SdfHelpers utils/test_SdfHelpers.cpp) target_link_libraries(test_SdfHelpers dart-utils sdformat::sdformat) +dart_add_test("unit" test_MeshLoader utils/test_MeshLoader.cpp) +target_link_libraries(test_MeshLoader dart-utils) + if(TARGET dart-io) dart_add_test("unit" test_IoRead io/test_Read.cpp) target_link_libraries(test_IoRead dart-io) diff --git a/tests/unit/dynamics/test_MeshShape.cpp b/tests/unit/dynamics/test_MeshShape.cpp index 0273d489b2c8d..e6c47290424b8 100644 --- a/tests/unit/dynamics/test_MeshShape.cpp +++ b/tests/unit/dynamics/test_MeshShape.cpp @@ -1,21 +1,31 @@ +#include "dart/common/Diagnostics.hpp" +#include "dart/common/Filesystem.hpp" #include "dart/common/LocalResourceRetriever.hpp" #include "dart/common/Uri.hpp" #include "dart/config.hpp" #include "dart/dynamics/ArrowShape.hpp" #include "dart/dynamics/AssimpInputResourceAdaptor.hpp" #include "dart/dynamics/MeshShape.hpp" +#include "dart/math/TriMesh.hpp" +#include "dart/utils/MeshLoader.hpp" #include #include #include +#include #include #include #include +#include #include #include +#include +#include #include +#include + using namespace dart; namespace { @@ -45,6 +55,15 @@ class AliasUriResourceRetriever final : public common::ResourceRetriever return mDelegate->retrieve(uri); } + std::string getFilePath(const common::Uri& uri) override + { + DART_SUPPRESS_DEPRECATED_BEGIN + if (isAlias(uri)) + return mDelegate->getFilePath(mTargetUri); + return mDelegate->getFilePath(uri); + DART_SUPPRESS_DEPRECATED_END + } + private: bool isAlias(const common::Uri& uri) const { @@ -78,6 +97,75 @@ class RecordingRetriever final : public common::ResourceRetriever std::string lastUri; }; +DART_SUPPRESS_DEPRECATED_BEGIN +class DirectAssignMeshShape final : public dynamics::MeshShape +{ +public: + DirectAssignMeshShape() + : dynamics::MeshShape(Eigen::Vector3d::Ones(), nullptr) + { + auto* scene = new aiScene(); + scene->mNumMeshes = 2; + scene->mMeshes = new aiMesh*[scene->mNumMeshes]; + + scene->mRootNode = new aiNode(); + scene->mRootNode->mNumMeshes = scene->mNumMeshes; + scene->mRootNode->mMeshes = new unsigned int[scene->mNumMeshes]; + scene->mRootNode->mMeshes[0] = 0u; + scene->mRootNode->mMeshes[1] = 1u; + + for (unsigned int meshIndex = 0; meshIndex < scene->mNumMeshes; + ++meshIndex) { + auto* mesh = new aiMesh(); + + mesh->mNumVertices = 3; + mesh->mVertices = new aiVector3D[mesh->mNumVertices]; + mesh->mNormals = new aiVector3D[mesh->mNumVertices]; + + const ai_real z = (meshIndex == 0 ? 0.0f : 1.0f); + + mesh->mVertices[0] = aiVector3D(0.0f, 0.0f, z); + mesh->mVertices[1] = aiVector3D(1.0f, 0.0f, z); + mesh->mVertices[2] = aiVector3D(0.0f, 1.0f, z); + + mesh->mNormals[0] = aiVector3D(0.0f, 0.0f, 1.0f); + mesh->mNormals[1] = aiVector3D(0.0f, 0.0f, 1.0f); + mesh->mNormals[2] = aiVector3D(0.0f, 0.0f, 1.0f); + + mesh->mNumFaces = 1; + mesh->mFaces = new aiFace[mesh->mNumFaces]; + mesh->mFaces[0].mNumIndices = 3; + mesh->mFaces[0].mIndices = new unsigned int[3]; + mesh->mFaces[0].mIndices[0] = 0u; + mesh->mFaces[0].mIndices[1] = 1u; + mesh->mFaces[0].mIndices[2] = 2u; + + mesh->mPrimitiveTypes = aiPrimitiveType_TRIANGLE; + + scene->mMeshes[meshIndex] = mesh; + } + + mRawScene = scene; + this->mMesh = scene; + this->mIsBoundingBoxDirty = true; + this->mIsVolumeDirty = true; + } + + const aiScene* rawScene() const + { + return mRawScene; + } + + MeshOwnership meshOwnership() const + { + return this->mMesh.getOwnership(); + } + +private: + const aiScene* mRawScene{nullptr}; +}; +DART_SUPPRESS_DEPRECATED_END + const aiScene* loadMeshWithOverrides( const std::string& uri, const common::ResourceRetrieverPtr& retriever, @@ -162,18 +250,26 @@ TEST(MeshShapeTest, CloneCreatesIndependentScene) ASSERT_FALSE(fileUri.empty()); auto retriever = std::make_shared(); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* scene = dynamics::MeshShape::loadMesh(fileUri, retriever); + DART_SUPPRESS_DEPRECATED_END ASSERT_NE(scene, nullptr); + DART_SUPPRESS_DEPRECATED_BEGIN auto original = std::make_shared( Eigen::Vector3d::Ones(), scene, fileUri, retriever); + DART_SUPPRESS_DEPRECATED_END const Eigen::Vector3d originalExtents = original->getBoundingBox().computeFullExtents(); auto cloned = std::dynamic_pointer_cast(original->clone()); ASSERT_NE(cloned, nullptr); + + DART_SUPPRESS_DEPRECATED_BEGIN EXPECT_NE(original->getMesh(), cloned->getMesh()); + DART_SUPPRESS_DEPRECATED_END + EXPECT_TRUE(cloned->getBoundingBox().computeFullExtents().isApprox( originalExtents, 1e-12)); @@ -199,19 +295,25 @@ TEST(MeshShapeTest, ColladaUnitMetadataApplied) auto retriever = std::make_shared(); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* sceneWithUnits = dynamics::MeshShape::loadMesh(fileUri, retriever); + DART_SUPPRESS_DEPRECATED_END ASSERT_NE(sceneWithUnits, nullptr); + DART_SUPPRESS_DEPRECATED_BEGIN const auto shapeWithUnits = std::make_shared( Eigen::Vector3d::Ones(), sceneWithUnits); + DART_SUPPRESS_DEPRECATED_END const Eigen::Vector3d extentsWithUnits = shapeWithUnits->getBoundingBox().computeFullExtents(); const aiScene* sceneIgnoringUnits = loadMeshWithOverrides(fileUri, retriever, true); ASSERT_NE(sceneIgnoringUnits, nullptr); + DART_SUPPRESS_DEPRECATED_BEGIN const auto shapeIgnoringUnits = std::make_shared( Eigen::Vector3d::Ones(), sceneIgnoringUnits); + DART_SUPPRESS_DEPRECATED_END const Eigen::Vector3d extentsIgnoringUnits = shapeIgnoringUnits->getBoundingBox().computeFullExtents(); @@ -231,20 +333,28 @@ TEST(MeshShapeTest, ColladaUriWithoutExtensionStillLoads) auto aliasRetriever = std::make_shared(aliasUri, filePath); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* aliasScene = dynamics::MeshShape::loadMesh(aliasUri, aliasRetriever); + DART_SUPPRESS_DEPRECATED_END ASSERT_NE(aliasScene, nullptr); + DART_SUPPRESS_DEPRECATED_BEGIN const auto aliasShape = std::make_shared( Eigen::Vector3d::Ones(), aliasScene); + DART_SUPPRESS_DEPRECATED_END const Eigen::Vector3d aliasExtents = aliasShape->getBoundingBox().computeFullExtents(); auto canonicalRetriever = std::make_shared(); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* canonicalScene = dynamics::MeshShape::loadMesh( common::Uri::createFromPath(filePath).toString(), canonicalRetriever); + DART_SUPPRESS_DEPRECATED_END ASSERT_NE(canonicalScene, nullptr); + DART_SUPPRESS_DEPRECATED_BEGIN const auto canonicalShape = std::make_shared( Eigen::Vector3d::Ones(), canonicalScene); + DART_SUPPRESS_DEPRECATED_END const Eigen::Vector3d canonicalExtents = canonicalShape->getBoundingBox().computeFullExtents(); @@ -264,13 +374,39 @@ TEST(MeshShapeTest, RespectsCustomMeshDeleter) delete const_cast(mesh); }); + DART_SUPPRESS_DEPRECATED_BEGIN dynamics::MeshShape shape(Eigen::Vector3d::Ones(), scene); + DART_SUPPRESS_DEPRECATED_END + + DART_SUPPRESS_DEPRECATED_BEGIN EXPECT_EQ(shape.getMesh(), scene.get()); + DART_SUPPRESS_DEPRECATED_END } EXPECT_EQ(deleted.load(), 1); } +TEST(MeshShapeTest, DirectAssignSceneBuildsTriMeshAndUsesDeleteSemantics) +{ + DirectAssignMeshShape shape; + EXPECT_EQ(shape.meshOwnership(), dynamics::MeshShape::MeshOwnership::Manual); + + DART_SUPPRESS_DEPRECATED_BEGIN + EXPECT_EQ(shape.getMesh(), shape.rawScene()); + DART_SUPPRESS_DEPRECATED_END + + const auto triMesh = shape.getTriMesh(); + ASSERT_NE(triMesh, nullptr); + EXPECT_EQ(triMesh->getVertices().size(), 6u); + EXPECT_EQ(triMesh->getTriangles().size(), 2u); + EXPECT_TRUE(triMesh->hasVertexNormals()); + + const Eigen::Vector3d extents = shape.getBoundingBox().computeFullExtents(); + EXPECT_NEAR(extents.x(), 1.0, 1e-12); + EXPECT_NEAR(extents.y(), 1.0, 1e-12); + EXPECT_NEAR(extents.z(), 1.0, 1e-12); +} + TEST(MeshShapeTest, TracksOwnershipAndUriMetadata) { auto retriever = std::make_shared(); @@ -278,45 +414,333 @@ TEST(MeshShapeTest, TracksOwnershipAndUriMetadata) = common::Uri::createFromStringOrPath("/tmp/manual-mesh.dae"); auto* manualScene = new aiScene; + + DART_SUPPRESS_DEPRECATED_BEGIN dynamics::MeshShape shape( Eigen::Vector3d::Ones(), manualScene, fileUri, retriever, dynamics::MeshShape::MeshOwnership::Manual); + DART_SUPPRESS_DEPRECATED_END + + DART_SUPPRESS_DEPRECATED_BEGIN EXPECT_EQ(shape.getMesh(), manualScene); + DART_SUPPRESS_DEPRECATED_END EXPECT_EQ(shape.getMeshPath(), fileUri.getFilesystemPath()); EXPECT_EQ(shape.getMeshUri(), fileUri.toString()); const common::Uri retrieverUri("package://example/mesh.dae"); auto* retrieverScene = new aiScene; + DART_SUPPRESS_DEPRECATED_BEGIN shape.setMesh( retrieverScene, dynamics::MeshShape::MeshOwnership::Manual, retrieverUri, retriever); + DART_SUPPRESS_DEPRECATED_END EXPECT_EQ(retriever->lastUri, retrieverUri.toString()); EXPECT_EQ(shape.getMeshPath(), "/virtual/path/from/retriever"); + + DART_SUPPRESS_DEPRECATED_BEGIN EXPECT_EQ(shape.getMesh(), retrieverScene); + DART_SUPPRESS_DEPRECATED_END // No-op when the mesh pointer and ownership are unchanged. + + DART_SUPPRESS_DEPRECATED_BEGIN shape.setMesh( shape.getMesh(), dynamics::MeshShape::MeshOwnership::Manual, retrieverUri, retriever); + DART_SUPPRESS_DEPRECATED_END // Clearing the mesh resets related metadata. + DART_SUPPRESS_DEPRECATED_BEGIN shape.setMesh( nullptr, dynamics::MeshShape::MeshOwnership::Manual, common::Uri(), nullptr); + DART_SUPPRESS_DEPRECATED_END + + DART_SUPPRESS_DEPRECATED_BEGIN EXPECT_EQ(shape.getMesh(), nullptr); + DART_SUPPRESS_DEPRECATED_END EXPECT_TRUE(shape.getMeshPath().empty()); EXPECT_TRUE(shape.getMeshUri().empty()); } +TEST(MeshShapeTest, TriMeshConstructorTracksUriMetadata) +{ + auto triMesh = std::make_shared>(); + triMesh->addVertex(0.0, 0.0, 0.0); + triMesh->addVertex(1.0, 0.0, 0.0); + triMesh->addVertex(0.0, 1.0, 0.0); + triMesh->addTriangle(0, 1, 2); + + const std::string filePath = dart::config::dataPath("skel/kima/l-foot.dae"); + const common::Uri fileUri = common::Uri::createFromPath(filePath); + + dynamics::MeshShape shape(Eigen::Vector3d::Ones(), triMesh, fileUri); + + EXPECT_EQ(shape.getMeshUri(), fileUri.toString()); + EXPECT_EQ(shape.getMeshPath(), fileUri.getFilesystemPath()); +} + +TEST(MeshShapeTest, TriMeshConstructorPreservesMaterialsFromUri) +{ + const std::string filePath = dart::config::dataPath("skel/kima/l-foot.dae"); + const std::string aliasUri = "collada-nodot://meshshape/lfoot"; + + auto aliasRetriever + = std::make_shared(aliasUri, filePath); + auto loader = std::make_unique(); + auto triMesh = loader->load(aliasUri, aliasRetriever); + ASSERT_NE(triMesh, nullptr); + + dynamics::MeshShape shape( + Eigen::Vector3d::Ones(), + std::move(triMesh), + common::Uri(aliasUri), + aliasRetriever); + + EXPECT_EQ(shape.getMeshUri(), aliasUri); + EXPECT_EQ(shape.getMeshPath(), filePath); + EXPECT_EQ(shape.getResourceRetriever(), aliasRetriever); + EXPECT_FALSE(shape.getMaterials().empty()); +} + +TEST(MeshShapeTest, TriMeshGetMeshCachesImportedScene) +{ + auto triMesh = std::make_shared>(); + triMesh->addVertex(0.0, 0.0, 0.0); + triMesh->addVertex(1.0, 0.0, 0.0); + triMesh->addVertex(0.0, 1.0, 0.0); + triMesh->addVertex(1.0, 1.0, 0.0); + triMesh->addTriangle(0, 1, 2); + triMesh->addTriangle(1, 3, 2); + + dynamics::MeshShape shape(Eigen::Vector3d::Ones(), triMesh, common::Uri()); + + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* sceneFirst = shape.getMesh(); + ASSERT_NE(sceneFirst, nullptr); + const aiScene* sceneSecond = shape.getMesh(); + DART_SUPPRESS_DEPRECATED_END + + EXPECT_EQ(sceneFirst, sceneSecond); + ASSERT_GE(sceneFirst->mNumMeshes, 1u); + ASSERT_NE(sceneFirst->mMeshes, nullptr); + ASSERT_NE(sceneFirst->mMeshes[0], nullptr); + EXPECT_EQ(sceneFirst->mMeshes[0]->mNumVertices, 4u); + EXPECT_EQ(sceneFirst->mMeshes[0]->mNumFaces, 2u); +} + +TEST(MeshShapeTest, TriMeshGetMeshPreservesTextureCoords) +{ + const auto tempDir = common::filesystem::temp_directory_path(); + const auto timestamp + = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + const auto baseName + = std::string("dart_meshshape_texcoords_") + std::to_string(timestamp); + const auto objPath = tempDir / (baseName + ".obj"); + + { + std::ofstream objFile(objPath.string()); + ASSERT_TRUE(objFile); + objFile << "v 0 0 0\n" + << "v 1 0 0\n" + << "v 0 1 0\n" + << "vt 0 0\n" + << "vt 1 0\n" + << "vt 0 1\n" + << "f 1/1 2/2 3/3\n"; + } + + auto retriever = std::make_shared(); + auto loader = std::make_unique(); + auto triMesh = loader->load(objPath.string(), retriever); + ASSERT_NE(triMesh, nullptr); + + dynamics::MeshShape shape( + Eigen::Vector3d::Ones(), + std::move(triMesh), + common::Uri::createFromPath(objPath.string()), + retriever); + + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* scene = shape.getMesh(); + DART_SUPPRESS_DEPRECATED_END + ASSERT_NE(scene, nullptr); + ASSERT_GE(scene->mNumMeshes, 1u); + const aiMesh* mesh = scene->mMeshes[0]; + ASSERT_NE(mesh, nullptr); + ASSERT_TRUE(mesh->HasTextureCoords(0)); + ASSERT_NE(mesh->mTextureCoords[0], nullptr); + EXPECT_EQ(mesh->mNumVertices, 3u); + + auto hasTexCoord = [mesh](float u, float v) { + for (unsigned int i = 0; i < mesh->mNumVertices; ++i) { + const auto& texCoord = mesh->mTextureCoords[0][i]; + if (std::fabs(texCoord.x - u) < 1e-6f + && std::fabs(texCoord.y - v) < 1e-6f) { + return true; + } + } + return false; + }; + + EXPECT_TRUE(hasTexCoord(0.0f, 0.0f)); + EXPECT_TRUE(hasTexCoord(1.0f, 0.0f)); + EXPECT_TRUE(hasTexCoord(0.0f, 1.0f)); + + common::error_code ec; + common::filesystem::remove(objPath, ec); +} + +TEST(MeshShapeTest, TriMeshGetMeshPreservesMaterialIndices) +{ + const auto tempDir = common::filesystem::temp_directory_path(); + const auto timestamp + = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + const auto baseName + = std::string("dart_meshshape_materials_") + std::to_string(timestamp); + const auto objPath = tempDir / (baseName + ".obj"); + const auto mtlPath = tempDir / (baseName + ".mtl"); + + { + std::ofstream mtlFile(mtlPath.string()); + ASSERT_TRUE(mtlFile); + mtlFile << "newmtl material_0\n" + << "Ka 0 0 0\n" + << "Kd 1 0 0\n" + << "Ks 0 0 0\n" + << "Ns 0\n" + << "newmtl material_1\n" + << "Ka 0 0 0\n" + << "Kd 0 1 0\n" + << "Ks 0 0 0\n" + << "Ns 0\n"; + } + + { + std::ofstream objFile(objPath.string()); + ASSERT_TRUE(objFile); + objFile << "mtllib " << mtlPath.filename().string() << "\n" + << "v 0 0 0\n" + << "v 1 0 0\n" + << "v 0 1 0\n" + << "v 1 1 0\n" + << "usemtl material_0\n" + << "f 1 2 3\n" + << "usemtl material_1\n" + << "f 2 4 3\n"; + } + + auto retriever = std::make_shared(); + auto loader = std::make_unique(); + auto triMesh = loader->load(objPath.string(), retriever); + ASSERT_NE(triMesh, nullptr); + + dynamics::MeshShape shape( + Eigen::Vector3d::Ones(), + std::move(triMesh), + common::Uri::createFromPath(objPath.string()), + retriever); + + EXPECT_GE(shape.getMaterials().size(), 2u); + + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* scene = shape.getMesh(); + DART_SUPPRESS_DEPRECATED_END + ASSERT_NE(scene, nullptr); + + ASSERT_GE(scene->mNumMaterials, 2u); + + std::set usedMaterialIndices; + std::set usedMaterialNames; + for (unsigned int i = 0; i < scene->mNumMeshes; ++i) { + const aiMesh* mesh = scene->mMeshes[i]; + ASSERT_NE(mesh, nullptr); + ASSERT_LT(mesh->mMaterialIndex, scene->mNumMaterials); + usedMaterialIndices.insert(mesh->mMaterialIndex); + + aiString materialName; + if (aiGetMaterialString( + scene->mMaterials[mesh->mMaterialIndex], + AI_MATKEY_NAME, + &materialName) + == aiReturn_SUCCESS) { + usedMaterialNames.insert(materialName.C_Str()); + } + } + + std::ostringstream materialSummary; + materialSummary << "indices="; + bool first = true; + for (const auto& index : usedMaterialIndices) { + if (!first) + materialSummary << ","; + materialSummary << index; + first = false; + } + materialSummary << " names="; + first = true; + for (const auto& name : usedMaterialNames) { + if (!first) + materialSummary << ","; + materialSummary << name; + first = false; + } + EXPECT_GE(usedMaterialIndices.size(), 2u) << materialSummary.str(); + EXPECT_GE(usedMaterialNames.size(), 2u) << materialSummary.str(); + + std::set expectedMaterialNames; + for (std::size_t index = 0; index < shape.getMaterials().size(); ++index) { + expectedMaterialNames.insert("material_" + std::to_string(index)); + } + for (const auto& name : usedMaterialNames) { + EXPECT_TRUE(expectedMaterialNames.count(name)) << materialSummary.str(); + } + + common::error_code ec; + common::filesystem::remove(objPath, ec); + common::filesystem::remove(mtlPath, ec); +} + +TEST(MeshShapeTest, SetMeshAdoptsCachedSceneWithoutUseAfterFree) +{ + auto triMesh = std::make_shared>(); + triMesh->addVertex(0.0, 0.0, 0.0); + triMesh->addVertex(1.0, 0.0, 0.0); + triMesh->addVertex(0.0, 1.0, 0.0); + triMesh->addTriangle(0, 1, 2); + + dynamics::MeshShape shape(Eigen::Vector3d::Ones(), triMesh, common::Uri()); + + DART_SUPPRESS_DEPRECATED_BEGIN + const aiScene* cachedScene = shape.getMesh(); + ASSERT_NE(cachedScene, nullptr); + + // Calling setMesh() with the cached aiScene pointer should not release the + // scene before adopting it (regression test for use-after-free). + shape.setMesh( + cachedScene, common::Uri("package://example/mesh.obj"), nullptr); + + const aiScene* adoptedScene = shape.getMesh(); + DART_SUPPRESS_DEPRECATED_END + + EXPECT_EQ(adoptedScene, cachedScene); + + const auto updatedTriMesh = shape.getTriMesh(); + ASSERT_NE(updatedTriMesh, nullptr); + EXPECT_TRUE(updatedTriMesh->hasTriangles()); + EXPECT_EQ(updatedTriMesh->getVertices().size(), 3u); + EXPECT_EQ(updatedTriMesh->getTriangles().size(), 1u); +} + TEST(ArrowShapeTest, CloneUsesMeshOwnershipSemantics) { dynamics::ArrowShape arrow( @@ -328,8 +752,11 @@ TEST(ArrowShapeTest, CloneUsesMeshOwnershipSemantics) auto cloned = std::dynamic_pointer_cast(arrow.clone()); ASSERT_TRUE(cloned); + + DART_SUPPRESS_DEPRECATED_BEGIN ASSERT_NE(cloned->getMesh(), nullptr); EXPECT_NE(cloned->getMesh(), arrow.getMesh()); + DART_SUPPRESS_DEPRECATED_END } TEST(ArrowShapeTest, MaterialCountIsInitialized) @@ -341,7 +768,9 @@ TEST(ArrowShapeTest, MaterialCountIsInitialized) Eigen::Vector4d::Ones(), 4); + DART_SUPPRESS_DEPRECATED_BEGIN const aiScene* scene = arrow.getMesh(); + DART_SUPPRESS_DEPRECATED_END ASSERT_NE(scene, nullptr); EXPECT_EQ(scene->mNumMaterials, 1u); ASSERT_NE(scene->mMaterials, nullptr); diff --git a/tests/unit/gui/test_MeshShapeNodeMaterialUpdates.cpp b/tests/unit/gui/test_MeshShapeNodeMaterialUpdates.cpp new file mode 100644 index 0000000000000..b48e6c11072b9 --- /dev/null +++ b/tests/unit/gui/test_MeshShapeNodeMaterialUpdates.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include + +#include +#include +#include + +TEST(MeshShapeNodeTest, UpdatesMaterialOnColorChange) +{ + const Eigen::Vector4d initialColor(0.1, 0.2, 0.3, 0.4); + auto arrow = std::make_shared( + Eigen::Vector3d::Zero(), + Eigen::Vector3d::UnitX(), + dart::dynamics::ArrowShape::Properties(), + initialColor); + arrow->setDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR); + + dart::dynamics::SimpleFrame frame; + frame.setShape(arrow); + frame.getVisualAspect(true)->setRGBA(initialColor); + + osg::ref_ptr frameNode + = new dart::gui::ShapeFrameNode(&frame, nullptr); + ASSERT_GT(frameNode->getNumChildren(), 0u); + auto* meshNode + = dynamic_cast(frameNode->getChild(0)); + ASSERT_NE(meshNode, nullptr); + + const auto* material = meshNode->getMaterial(0); + ASSERT_NE(material, nullptr); + osg::Vec4 diffuse = material->getDiffuse(osg::Material::FRONT_AND_BACK); + EXPECT_FLOAT_EQ(diffuse.r(), static_cast(initialColor[0])); + EXPECT_FLOAT_EQ(diffuse.g(), static_cast(initialColor[1])); + EXPECT_FLOAT_EQ(diffuse.b(), static_cast(initialColor[2])); + EXPECT_FLOAT_EQ(diffuse.a(), static_cast(initialColor[3])); + + const Eigen::Vector4d updatedColor(0.8, 0.7, 0.6, 0.5); + frame.getVisualAspect()->setRGBA(updatedColor); + meshNode->refresh(); + + material = meshNode->getMaterial(0); + ASSERT_NE(material, nullptr); + diffuse = material->getDiffuse(osg::Material::FRONT_AND_BACK); + EXPECT_FLOAT_EQ(diffuse.r(), static_cast(updatedColor[0])); + EXPECT_FLOAT_EQ(diffuse.g(), static_cast(updatedColor[1])); + EXPECT_FLOAT_EQ(diffuse.b(), static_cast(updatedColor[2])); + EXPECT_FLOAT_EQ(diffuse.a(), static_cast(updatedColor[3])); +} diff --git a/tests/unit/math/test_TriMesh.cpp b/tests/unit/math/test_TriMesh.cpp index fb73d193e7ee0..75e386195b62f 100644 --- a/tests/unit/math/test_TriMesh.cpp +++ b/tests/unit/math/test_TriMesh.cpp @@ -147,3 +147,65 @@ TEST(TriMeshTests, GenerateConvexHull) EXPECT_EQ(convexHull->getVertices().size(), vertices.size()); EXPECT_EQ(convexHull->getTriangles().size(), 4); } + +//============================================================================== +TEST(TriMeshTests, ReserveAndAdd) +{ + auto mesh = TriMeshd(); + + mesh.reserveVertices(3); + mesh.reserveTriangles(1); + mesh.reserveVertexNormals(3); + + mesh.addVertex(0.0, 0.0, 0.0); + mesh.addVertex(1.0, 0.0, 0.0); + mesh.addVertex(0.0, 1.0, 0.0); + mesh.addTriangle(0, 1, 2); + + EXPECT_EQ(mesh.getVertices().size(), 3u); + EXPECT_EQ(mesh.getTriangles().size(), 1u); + EXPECT_FALSE(mesh.hasVertexNormals()); + + mesh.addVertexNormal(0.0, 0.0, 1.0); + mesh.addVertexNormal(0.0, 0.0, 1.0); + mesh.addVertexNormal(0.0, 0.0, 1.0); + EXPECT_TRUE(mesh.hasVertexNormals()); + EXPECT_EQ(mesh.getVertexNormals().size(), 3u); + + Eigen::Vector3d vertex(0.5, 0.5, 0.0); + mesh.addVertex(vertex); + EXPECT_FALSE(mesh.hasVertexNormals()); + EXPECT_EQ(mesh.getVertices().size(), 4u); + + Eigen::Matrix triangle; + triangle << 0, 1, 3; + mesh.addTriangle(triangle); + EXPECT_EQ(mesh.getTriangles().size(), 2u); + + Eigen::Vector3d normal(0.0, 1.0, 0.0); + mesh.addVertexNormal(normal); + EXPECT_TRUE(mesh.hasVertexNormals()); + EXPECT_EQ(mesh.getVertexNormals().size(), 4u); +} + +//============================================================================== +TEST(TriMeshTests, ComputeVertexNormals) +{ + auto mesh = TriMeshd(); + + mesh.addVertex(0.0, 0.0, 0.0); + mesh.addVertex(1.0, 0.0, 0.0); + mesh.addVertex(0.0, 1.0, 0.0); + mesh.addTriangle(0, 1, 2); + + mesh.computeVertexNormals(); + + EXPECT_TRUE(mesh.hasVertexNormals()); + EXPECT_EQ(mesh.getVertexNormals().size(), mesh.getVertices().size()); + + for (const auto& normal : mesh.getVertexNormals()) { + EXPECT_NEAR(normal.z(), 1.0, 1e-10); + EXPECT_NEAR(normal.x(), 0.0, 1e-10); + EXPECT_NEAR(normal.y(), 0.0, 1e-10); + } +} diff --git a/tests/unit/utils/test_MeshLoader.cpp b/tests/unit/utils/test_MeshLoader.cpp new file mode 100644 index 0000000000000..4624e4601a6fd --- /dev/null +++ b/tests/unit/utils/test_MeshLoader.cpp @@ -0,0 +1,119 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * This file provides unit tests for the new MeshLoader abstraction + */ + +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +using namespace dart; + +//============================================================================== +TEST(MeshLoader, AssimpMeshLoaderBasic) +{ + // Create an AssimpMeshLoader + auto loader = std::make_unique(); + ASSERT_NE(loader, nullptr); +} + +//============================================================================== +TEST(MeshLoader, LoadNonExistentFile) +{ + auto loader = std::make_unique(); + auto retriever = std::make_shared(); + + // Try to load a non-existent file + auto mesh = loader->load("non_existent_file.dae", retriever); + EXPECT_EQ(mesh, nullptr); +} + +//============================================================================== +// Test actual file loading with a real mesh file from the repository +TEST(MeshLoader, LoadActualMesh) +{ + auto loader = std::make_unique(); + auto retriever = utils::DartResourceRetriever::create(); + + // Use DART resource URI for the sample mesh file + // This uses the "dart://sample/obj/BoxSmall.obj" pattern that DART tests use + std::string meshUri = "dart://sample/obj/BoxSmall.obj"; + auto mesh = loader->load(meshUri, retriever); + + ASSERT_NE(mesh, nullptr) << "Failed to load mesh from " << meshUri; + + // Verify the mesh has data + EXPECT_GT(mesh->getVertices().size(), 0); + EXPECT_GT(mesh->getTriangles().size(), 0); + EXPECT_TRUE(mesh->hasVertices()); + EXPECT_TRUE(mesh->hasTriangles()); + + // BoxSmall.obj is a simple box, should have reasonable number of vertices + EXPECT_GE(mesh->getVertices().size(), 8); // At least 8 vertices for a box + EXPECT_GE( + mesh->getTriangles().size(), + 12); // At least 12 triangles for a box (6 faces * 2) + + // Verify normals are computed or loaded + if (mesh->hasVertexNormals()) { + EXPECT_EQ(mesh->getVertexNormals().size(), mesh->getVertices().size()); + } +} + +//============================================================================== +TEST(MeshLoader, MergesMultipleMeshes) +{ + const std::string meshPath = dart::config::dataPath("skel/kima/thorax.dae"); + ASSERT_FALSE(meshPath.empty()); + + auto loader = std::make_unique(); + auto mesh = loader->load(meshPath); + + ASSERT_NE(mesh, nullptr) << "Failed to load mesh from " << meshPath; + + const unsigned int flags = aiProcess_GenNormals | aiProcess_Triangulate + | aiProcess_JoinIdenticalVertices + | aiProcess_SortByPType | aiProcess_OptimizeMeshes; + + const aiScene* scene = aiImportFile(meshPath.c_str(), flags); + ASSERT_NE(scene, nullptr) << "Assimp error: " << aiGetErrorString(); + + scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices); + ASSERT_NE(scene, nullptr); + + ASSERT_GT(scene->mNumMeshes, 1u) + << "Expected an asset with multiple meshes to validate merge behavior."; + + std::size_t expectedVertices = 0; + std::size_t expectedTriangles = 0; + for (std::size_t meshIndex = 0; meshIndex < scene->mNumMeshes; ++meshIndex) { + const aiMesh* assimpMesh = scene->mMeshes[meshIndex]; + ASSERT_NE(assimpMesh, nullptr); + + expectedVertices += assimpMesh->mNumVertices; + for (unsigned int faceIndex = 0; faceIndex < assimpMesh->mNumFaces; + ++faceIndex) { + if (assimpMesh->mFaces[faceIndex].mNumIndices == 3u) { + ++expectedTriangles; + } + } + } + + EXPECT_EQ(mesh->getVertices().size(), expectedVertices); + EXPECT_EQ(mesh->getTriangles().size(), expectedTriangles); + EXPECT_TRUE(mesh->hasVertexNormals()); + EXPECT_EQ(mesh->getVertexNormals().size(), mesh->getVertices().size()); + + aiReleaseImport(scene); +}