Support Collada unit scaling in MeshShape

CHANGELOG.md

@@ -39,6 +39,7 @@
   * Removed all APIs deprecated in DART 6.12 (the `SdfParser::readWorld`/`readSkeleton` overloads that accepted direct `ResourceRetriever` parameters).
   * Removed all APIs deprecated in DART 6.13 (the legacy `dart::common::Timer` utility, `ConstraintSolver::getConstraints()`/`containSkeleton()`, `ContactConstraint`'s raw constructor and material helper statics, and the `MetaSkeleton` vector-returning `getBodyNodes()`/`getJoints()` accessors).
   * Removed the remaining 6.13 compatibility shims: deleted `dart/utils/urdf/URDFTypes.hpp`, the Eigen alias typedefs in `math/MathTypes.hpp`, the `dart7::comps::NameComponent` alias, and the legacy `dInfinity`/`dPAD` helpers, and tightened `SkelParser` plane parsing to treat `<point>` as an error.
+  * Fixed Collada mesh imports ignoring `<unit>` metadata by preserving the Assimp-provided scale transform ([#287](https://github.com/dartsim/dart/issues/287)).
 
 * Tutorials
   * Added explicit placeholder bodies to unfinished domino and biped Python tutorials so users can import/run the scaffolds without `IndentationError`s.

dart/dynamics/MeshShape.cpp

@@ -41,8 +41,10 @@
 
 #include <assimp/Importer.hpp>
 #include <assimp/cimport.h>
+#include <assimp/config.h>
 #include <assimp/postprocess.h>
 
+#include <algorithm>
 #include <limits>
 #include <string>
 
@@ -550,16 +552,60 @@ aiScene* MeshShape::cloneMesh() const
 }
 
 //==============================================================================
+namespace {
+
+bool hasColladaExtension(const std::string& path)
+{
+  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";
+}
+
+bool isColladaResource(
+    const std::string& uri, const common::ResourceRetrieverPtr& retriever)
+{
+  if (hasColladaExtension(uri))
+    return true;
+
+  if (retriever) {
+    const auto parsedUri = common::Uri::createFromStringOrPath(uri);
+    if (parsedUri.mPath) {
+      const std::string resolvedPath = retriever->getFilePath(parsedUri);
+      if (!resolvedPath.empty())
+        return hasColladaExtension(resolvedPath);
+    }
+  }
+
+  return false;
+}
+
+} // namespace
+
 const aiScene* MeshShape::loadMesh(
     const std::string& _uri, const common::ResourceRetrieverPtr& retriever)
 {
+  const bool isCollada = isColladaResource(_uri, retriever);
+
   // 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
+
   // 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...
@@ -583,24 +629,10 @@ const aiScene* MeshShape::loadMesh(
     return nullptr;
   }
 
-  // Assimp rotates collada files such that the up-axis (specified in the
-  // collada file) aligns with assimp's y-axis. Here we are reverting this
-  // rotation. We are only catching files with the .dae file ending here. We
-  // might miss files with an .xml file ending, which would need to be looked
-  // into to figure out whether they are collada files.
-  std::string extension;
-  const std::size_t extensionIndex = _uri.find_last_of('.');
-  if (extensionIndex != std::string::npos)
-    extension = _uri.substr(extensionIndex);
-
-  std::transform(
-      std::begin(extension),
-      std::end(extension),
-      std::begin(extension),
-      ::tolower);
-
-  if (extension == ".dae" || extension == ".zae")
+#if !defined(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION)
+  if (isCollada && scene->mRootNode)
     scene->mRootNode->mTransformation = aiMatrix4x4();
+#endif
 
   // Finally, pre-transform the vertices. We can't do this as part of the
   // import process, because we may have changed mTransformation above.

tests/unit/CMakeLists.txt

@@ -58,6 +58,7 @@ dart_add_test("unit" test_GenericJoints dynamics/test_GenericJoints.cpp)
 dart_add_test("unit" test_Inertia dynamics/test_Inertia.cpp)
 dart_add_test("unit" test_ScrewJoint dynamics/test_ScrewJoint.cpp)
 dart_add_test("unit" test_ShapeNodePtr dynamics/test_ShapeNodePtr.cpp)
+dart_add_test("unit" test_MeshShape dynamics/test_MeshShape.cpp)
 dart_add_test("unit" test_BodyNodeDerivatives dynamics/test_BodyNodeDerivatives.cpp)
 dart_add_test("unit" test_ShapeNodeInertia dynamics/test_ShapeNodeInertia.cpp)
 

tests/unit/dynamics/test_MeshShape.cpp

@@ -0,0 +1,204 @@
+#include "dart/common/LocalResourceRetriever.hpp"
+#include "dart/common/Uri.hpp"
+#include "dart/config.hpp"
+#include "dart/dynamics/AssimpInputResourceAdaptor.hpp"
+#include "dart/dynamics/MeshShape.hpp"
+
+#include <assimp/cimport.h>
+#include <assimp/config.h>
+#include <assimp/postprocess.h>
+#include <gtest/gtest.h>
+
+#include <fstream>
+#include <memory>
+#include <string>
+
+using namespace dart;
+
+namespace {
+
+class AliasUriResourceRetriever final : public common::ResourceRetriever
+{
+public:
+  AliasUriResourceRetriever(
+      const std::string& aliasUri, const std::string& targetFilePath)
+    : mAliasUri(aliasUri),
+      mTargetUri(common::Uri::createFromPath(targetFilePath)),
+      mDelegate(std::make_shared<common::LocalResourceRetriever>())
+  {
+  }
+
+  bool exists(const common::Uri& uri) override
+  {
+    if (isAlias(uri))
+      return mDelegate->exists(mTargetUri);
+    return mDelegate->exists(uri);
+  }
+
+  common::ResourcePtr retrieve(const common::Uri& uri) override
+  {
+    if (isAlias(uri))
+      return mDelegate->retrieve(mTargetUri);
+    return mDelegate->retrieve(uri);
+  }
+
+  std::string getFilePath(const common::Uri& uri) override
+  {
+    if (isAlias(uri))
+      return mDelegate->getFilePath(mTargetUri);
+    return mDelegate->getFilePath(uri);
+  }
+
+private:
+  bool isAlias(const common::Uri& uri) const
+  {
+    return uri.toString() == mAliasUri;
+  }
+
+  std::string mAliasUri;
+  common::Uri mTargetUri;
+  common::LocalResourceRetrieverPtr mDelegate;
+};
+
+const aiScene* loadMeshWithOverrides(
+    const std::string& uri,
+    const common::ResourceRetrieverPtr& retriever,
+    bool ignoreUnitSize)
+{
+  aiPropertyStore* propertyStore = aiCreatePropertyStore();
+  aiSetImportPropertyInteger(
+      propertyStore,
+      AI_CONFIG_PP_SBP_REMOVE,
+      aiPrimitiveType_POINT | aiPrimitiveType_LINE);
+
+#ifdef AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION
+  aiSetImportPropertyInteger(
+      propertyStore, AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
+#endif
+
+#ifdef AI_CONFIG_IMPORT_COLLADA_IGNORE_UNIT_SIZE
+  if (ignoreUnitSize) {
+    aiSetImportPropertyInteger(
+        propertyStore, AI_CONFIG_IMPORT_COLLADA_IGNORE_UNIT_SIZE, 1);
+  }
+#else
+  if (ignoreUnitSize) {
+    aiReleasePropertyStore(propertyStore);
+    return nullptr;
+  }
+#endif
+
+  dynamics::AssimpInputResourceRetrieverAdaptor systemIO(retriever);
+  aiFileIO fileIO = dynamics::createFileIO(&systemIO);
+
+  const aiScene* scene = aiImportFileExWithProperties(
+      uri.c_str(),
+      aiProcess_GenNormals | aiProcess_Triangulate
+          | aiProcess_JoinIdenticalVertices | aiProcess_SortByPType
+          | aiProcess_OptimizeMeshes,
+      &fileIO,
+      propertyStore);
+
+  if (!scene) {
+    aiReleasePropertyStore(propertyStore);
+    return nullptr;
+  }
+
+  scene = aiApplyPostProcessing(scene, aiProcess_PreTransformVertices);
+  aiReleasePropertyStore(propertyStore);
+  return scene;
+}
+
+double readColladaUnitScale(const std::string& path)
+{
+  std::ifstream file(path);
+  if (!file.is_open())
+    return 1.0;
+
+  const std::string token = "meter=\"";
+  std::string buffer(
+      (std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
+
+  const std::size_t pos = buffer.find(token);
+  if (pos == std::string::npos)
+    return 1.0;
+
+  const std::size_t start = pos + token.size();
+  const std::size_t end = buffer.find('"', start);
+  if (end == std::string::npos)
+    return 1.0;
+
+  try {
+    return std::stod(buffer.substr(start, end - start));
+  } catch (...) {
+    return 1.0;
+  }
+}
+
+} // namespace
+
+TEST(MeshShapeTest, ColladaUnitMetadataApplied)
+{
+#ifndef AI_CONFIG_IMPORT_COLLADA_IGNORE_UNIT_SIZE
+  GTEST_SKIP() << "Assimp build does not expose unit-size control property.";
+#endif
+
+  const std::string filePath = dart::config::dataPath("skel/kima/l-foot.dae");
+  const std::string fileUri = common::Uri::createFromPath(filePath).toString();
+  ASSERT_FALSE(fileUri.empty());
+
+  auto retriever = std::make_shared<common::LocalResourceRetriever>();
+
+  const aiScene* sceneWithUnits
+      = dynamics::MeshShape::loadMesh(fileUri, retriever);
+  ASSERT_NE(sceneWithUnits, nullptr);
+  const auto shapeWithUnits = std::make_shared<dynamics::MeshShape>(
+      Eigen::Vector3d::Ones(), sceneWithUnits);
+  const Eigen::Vector3d extentsWithUnits
+      = shapeWithUnits->getBoundingBox().computeFullExtents();
+
+  const aiScene* sceneIgnoringUnits
+      = loadMeshWithOverrides(fileUri, retriever, true);
+  ASSERT_NE(sceneIgnoringUnits, nullptr);
+  const auto shapeIgnoringUnits = std::make_shared<dynamics::MeshShape>(
+      Eigen::Vector3d::Ones(), sceneIgnoringUnits);
+  const Eigen::Vector3d extentsIgnoringUnits
+      = shapeIgnoringUnits->getBoundingBox().computeFullExtents();
+
+  const double unitScale = readColladaUnitScale(filePath);
+  ASSERT_GT(unitScale, 0.0);
+
+  EXPECT_TRUE(extentsWithUnits.isApprox(extentsIgnoringUnits * unitScale, 1e-6))
+      << "extentsWithUnits=" << extentsWithUnits.transpose()
+      << ", extentsIgnoringUnits=" << extentsIgnoringUnits.transpose()
+      << ", unitScale=" << unitScale;
+}
+
+TEST(MeshShapeTest, ColladaUriWithoutExtensionStillLoads)
+{
+  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<AliasUriResourceRetriever>(aliasUri, filePath);
+  const aiScene* aliasScene
+      = dynamics::MeshShape::loadMesh(aliasUri, aliasRetriever);
+  ASSERT_NE(aliasScene, nullptr);
+  const auto aliasShape = std::make_shared<dynamics::MeshShape>(
+      Eigen::Vector3d::Ones(), aliasScene);
+  const Eigen::Vector3d aliasExtents
+      = aliasShape->getBoundingBox().computeFullExtents();
+
+  auto canonicalRetriever = std::make_shared<common::LocalResourceRetriever>();
+  const aiScene* canonicalScene = dynamics::MeshShape::loadMesh(
+      common::Uri::createFromPath(filePath).toString(), canonicalRetriever);
+  ASSERT_NE(canonicalScene, nullptr);
+  const auto canonicalShape = std::make_shared<dynamics::MeshShape>(
+      Eigen::Vector3d::Ones(), canonicalScene);
+  const Eigen::Vector3d canonicalExtents
+      = canonicalShape->getBoundingBox().computeFullExtents();
+
+  EXPECT_TRUE(aliasExtents.isApprox(canonicalExtents, 1e-6))
+      << "aliasExtents=" << aliasExtents.transpose()
+      << ", canonicalExtents=" << canonicalExtents.transpose();
+}