From 774a5ec241c9487031ea20a30604bae325dd09ab Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Wed, 12 Jan 2022 10:10:57 +0000 Subject: [PATCH 1/4] polygon triangulation using the fan-triangulation algorithm Signed-off-by: Teo Koon Peng --- src/usd/usd_parser/polygon_helper.cc | 85 +++++++--------------------- src/usd/usd_parser/polygon_helper.hh | 8 +++ 2 files changed, 28 insertions(+), 65 deletions(-) diff --git a/src/usd/usd_parser/polygon_helper.cc b/src/usd/usd_parser/polygon_helper.cc index ecac569a5..923664b99 100644 --- a/src/usd/usd_parser/polygon_helper.cc +++ b/src/usd/usd_parser/polygon_helper.cc @@ -17,82 +17,37 @@ #include "polygon_helper.hh" -#include -#include -#include -#include #include #include -typedef CGAL::Exact_predicates_inexact_constructions_kernel K; -typedef CGAL::Triangulation_vertex_base_with_info_3 Vb; -typedef CGAL::Delaunay_triangulation_cell_base_3 Cb; -typedef CGAL::Triangulation_data_structure_3 Tds; -typedef CGAL::Delaunay_triangulation_3 Delaunay; -typedef Delaunay::Point Point; - namespace usd { std::vector PolygonToTriangles( - pxr::VtIntArray &_faceVertexIndices, - pxr::VtIntArray &_faceVertexCounts, - pxr::VtArray &_points) + pxr::VtIntArray &_faceVertexIndices, + pxr::VtIntArray &_faceVertexCounts, + pxr::VtArray &_points) { - std::vector indices; + size_t count = 0; + for (const auto &vCount : _faceVertexCounts) + { + count += vCount - 2; + } + std::vector triangles; + triangles.reserve(count * 3); - unsigned int indexVertex = 0; - for (unsigned int i = 0; i < _faceVertexCounts.size(); ++i) + size_t cur = 0; + for (const auto &vCount : _faceVertexCounts) { - if (_faceVertexCounts[i] == 3) - { - unsigned int j = indexVertex; - unsigned int indexVertexStop = indexVertex + 3; - for (; j < indexVertexStop; ++j) - { - indices.emplace_back(_faceVertexIndices[j]); - ++indexVertex; - } - } - else + for (size_t i = cur + 2; i < cur + vCount; i++) { - std::vector< std::pair > P; - for (int j = 0; j < _faceVertexCounts[i]; ++j) - { - pxr::GfVec3f & _p = _points[_faceVertexIndices[indexVertex]]; - P.push_back( - std::make_pair( - Point(_p[0], _p[1], _p[2]), - _faceVertexIndices[indexVertex])); - ++indexVertex; - } - Delaunay triangulation(P.begin(), P.end()); - - bool goodTriangle = true; - for(Delaunay::Finite_facets_iterator fit = triangulation.finite_facets_begin(); - fit != triangulation.finite_facets_end(); ++fit) - { - auto &face = fit; - if (face->first->vertex(0)->info() > maxIndex || - face->first->vertex(1)->info() > maxIndex || - face->first->vertex(2)->info() > maxIndex) - { - goodTriangle = false; - } - } - - if (goodTriangle) - { - for(Delaunay::Finite_facets_iterator fit = triangulation.finite_facets_begin(); - fit != triangulation.finite_facets_end(); ++fit) - { - auto &face = fit; - indices.emplace_back(face->first->vertex(0)->info()); - indices.emplace_back(face->first->vertex(1)->info()); - indices.emplace_back(face->first->vertex(2)->info()); - } - } + triangles.emplace_back(_faceVertexIndices[cur]); + triangles.emplace_back(_faceVertexIndices[i - 1]); + triangles.emplace_back(_faceVertexIndices[i]); } + cur += vCount; } - return indices; + assert(triangles.size() == count * 3); + + return triangles; } } diff --git a/src/usd/usd_parser/polygon_helper.hh b/src/usd/usd_parser/polygon_helper.hh index 2ebe5e069..c956ed351 100644 --- a/src/usd/usd_parser/polygon_helper.hh +++ b/src/usd/usd_parser/polygon_helper.hh @@ -20,8 +20,16 @@ #include #include + namespace usd { + ///\brief Converts a vertex indicies of a polygon mesh to a vertex indicies of a triangle mesh. + ///\details This uses the fan-triangulating algorithm, so it only works if all polygons in + /// in the mesh are convex. + ///\param _faceVertexIndices A flat list of vertex indicies of a polygon mesh. + ///\param _faceVertexCounts A list containing the number of vertices for each face of the mesh. + ///\param _points A list of vertices. + ///\return A flat list of vertex indicies, with each face converted to one or more triangles. std::vector PolygonToTriangles( pxr::VtIntArray &_faceVertexIndices, pxr::VtIntArray &_faceVertexCounts, From 15871a05f3c1e76731a9ae657032094e0bac9b55 Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Thu, 13 Jan 2022 04:06:55 +0000 Subject: [PATCH 2/4] remove cgal dep Signed-off-by: Teo Koon Peng --- src/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8ff92096e..15161feb0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -96,7 +96,6 @@ else() endif() find_package(pxr REQUIRED) -find_package(CGAL) include_directories(${PXR_INCLUDE_DIRS}) set(sources ${sources} @@ -265,7 +264,6 @@ target_link_libraries(${sdf_target} ignition-common${IGN_COMMON_VER}::graphics ${PYTHON_LIBRARY} ${PXR_LIBRARIES} - CGAL::CGAL PRIVATE ${TinyXML2_LIBRARIES}) From dedca340308a4a34d479de745262aa8b46d3819d Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Thu, 13 Jan 2022 04:15:09 +0000 Subject: [PATCH 3/4] add todo Signed-off-by: Teo Koon Peng --- src/usd/usd_parser/polygon_helper.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/usd/usd_parser/polygon_helper.cc b/src/usd/usd_parser/polygon_helper.cc index 923664b99..88efec683 100644 --- a/src/usd/usd_parser/polygon_helper.cc +++ b/src/usd/usd_parser/polygon_helper.cc @@ -27,6 +27,10 @@ namespace usd pxr::VtIntArray &_faceVertexCounts, pxr::VtArray &_points) { + // TODO: Use more robust algorithms. + // For reference, blender supports "ear-clipping", and "Beauty". + // ref: https://blender.stackexchange.com/questions/215553/what-algorithm-is-used-for-beauty-triangulation + // ref: https://en.wikipedia.org/wiki/Polygon_triangulation size_t count = 0; for (const auto &vCount : _faceVertexCounts) { From 6547ea7f87596f20fa70abfef320244023f69dbd Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Tue, 18 Jan 2022 20:58:10 +0800 Subject: [PATCH 4/4] fix xformOp:transform parsing (#822) Signed-off-by: Teo Koon Peng --- src/usd/usd_parser/utils.cc | 113 +++++++----------------------------- 1 file changed, 21 insertions(+), 92 deletions(-) diff --git a/src/usd/usd_parser/utils.cc b/src/usd/usd_parser/utils.cc index bd40e33d6..c0ff4a7c6 100644 --- a/src/usd/usd_parser/utils.cc +++ b/src/usd/usd_parser/utils.cc @@ -359,6 +359,12 @@ namespace usd } pose.Pos() = t.translate * metersPerUnit; + // scaling is lost when we convert to pose, so we pre-scale the translation + // to make them match the scaled values. + if (!_tfs.empty()) { + auto& child = _tfs.back(); + child.Pos().Set(child.Pos().X() * t.scale[0], child.Pos().Y() * t.scale[1], child.Pos().Z() * t.scale[2]); + } if (!t.isRotationZYX) { @@ -560,105 +566,28 @@ namespace usd if (op == "xformOp:transform") { + // FIXME: Shear is lost (does sdformat support it?). + pxr::GfMatrix4d transform; _prim.GetAttribute(pxr::TfToken("xformOp:transform")).Get(&transform); - pxr::GfVec3d translateMatrix = transform.ExtractTranslation(); - pxr::GfQuatd rotation_quadMatrix = transform.ExtractRotationQuat(); - - ignition::math::Matrix4d m( - transform[0][0], transform[0][1], transform[0][2], transform[0][3], - transform[1][0], transform[1][1], transform[1][2], transform[1][3], - transform[2][0], transform[2][1], transform[2][2], transform[2][3], - transform[3][0], transform[3][1], transform[3][2], transform[3][3] - ); - std::cerr << "m " << m << '\n'; + const auto rot = transform.RemoveScaleShear(); + const auto scaleShear = transform * rot.GetInverse(); - std::pair> data = - _usdData.findStage(_prim.GetPath().GetName()); + t.scale[0] = scaleShear[0][0]; + t.scale[1] = scaleShear[1][1]; + t.scale[2] = scaleShear[2][2]; - // bool upAxisZ = (data.second->_upAxis == "Z"); - // if(!upAxisZ) - // { - // ignition::math::Matrix4d mUpAxis( - // 1, 0, 0, 0, - // 0, 0, -1, 0, - // 0, 1, 0, 0, - // 0, 0, 0, 1); - // - // pxr::GfMatrix4d mUpAxis2( - // 1, 0, 0, 0, - // 0, 0, -1, 0, - // 0, 1, 0, 0, - // 0, 0, 0, 1); - // - // m = mUpAxis * m; - // transform = mUpAxis2 * transform; - // std::cerr << "m upAxis " << m << '\n'; - // } - - ignition::math::Vector3d eulerAngles = m.EulerRotation(true); - std::cerr << "eulerAngles " << eulerAngles << '\n'; - ignition::math::Matrix4d inverseRX(ignition::math::Pose3d( - ignition::math::Vector3d(0, 0, 0), - ignition::math::Quaterniond(-eulerAngles[0], 0, 0))); - ignition::math::Matrix4d inverseRY(ignition::math::Pose3d( - ignition::math::Vector3d(0, 0, 0), - ignition::math::Quaterniond(0, -eulerAngles[1], 0))); - ignition::math::Matrix4d inverseRZ(ignition::math::Pose3d( - ignition::math::Vector3d(0, 0, 0), - ignition::math::Quaterniond(0, 0, -eulerAngles[2]))); - - pxr::GfMatrix4d inverseR2X( - inverseRX(0, 0), inverseRX(0, 1), inverseRX(0, 2), inverseRX(0, 3), - inverseRX(1, 0), inverseRX(1, 1), inverseRX(1, 2), inverseRX(1, 3), - inverseRX(2, 0), inverseRX(2, 1), inverseRX(2, 2), inverseRX(2, 3), - inverseRX(3, 0), inverseRX(3, 1), inverseRX(3, 2), inverseRX(3, 3)); - pxr::GfMatrix4d inverseR2Y( - inverseRY(0, 0), inverseRY(0, 1), inverseRY(0, 2), inverseRY(0, 3), - inverseRY(1, 0), inverseRY(1, 1), inverseRY(1, 2), inverseRY(1, 3), - inverseRY(2, 0), inverseRY(2, 1), inverseRY(2, 2), inverseRY(2, 3), - inverseRY(3, 0), inverseRY(3, 1), inverseRY(3, 2), inverseRY(3, 3)); - pxr::GfMatrix4d inverseR2Z( - inverseRZ(0, 0), inverseRZ(0, 1), inverseRZ(0, 2), inverseRZ(0, 3), - inverseRZ(1, 0), inverseRZ(1, 1), inverseRZ(1, 2), inverseRZ(1, 3), - inverseRZ(2, 0), inverseRZ(2, 1), inverseRZ(2, 2), inverseRZ(2, 3), - inverseRZ(3, 0), inverseRZ(3, 1), inverseRZ(3, 2), inverseRZ(3, 3)); - - m = inverseRX * (inverseRY * (inverseRZ * m)); - transform = inverseR2X * (inverseR2Y * (inverseR2Z * transform)); - // ignition::math::Vector3d t = m.Translation(); - // ignition::math::Vector3d euler = m.EulerRotation(true); - // ignition::math::Quaternion r(eulerAngles[0], eulerAngles[1], eulerAngles[2]); - std::cerr << "m " << m << '\n'; - - // std::cerr << "m " << m << '\n'; - // std::cerr << "transform " << transform << '\n'; - - t.scale[0] = transform[0][0]; - t.scale[1] = transform[1][1]; - t.scale[2] = transform[2][2]; - - pxr::GfVec3d translateVector = transform.ExtractTranslation(); - pxr::GfQuatd rotationInversed = transform.ExtractRotationQuat(); - t.translate = ignition::math::Vector3d( - translateVector[0], translateVector[1], translateVector[2]); - ignition::math::Quaterniond q(eulerAngles[0], eulerAngles[1], eulerAngles[2]); + const auto rotQuat = rot.ExtractRotationQuat(); + t.translate = ignition::math::Vector3d(transform[3][0], transform[3][1], transform[3][2]); + ignition::math::Quaterniond q( + rotQuat.GetReal(), + rotQuat.GetImaginary()[0], + rotQuat.GetImaginary()[1], + rotQuat.GetImaginary()[2] + ); t.q.push_back(q); - // translate[0] = translateVector[0]; - // translate[1] = translateVector[1]; - // translate[2] = translateVector[2]; - // rotationQuad.SetImaginary(r.X(), r.Y(), r.Z()); - // rotationQuad.SetReal(r.W()); - - // isTranslate = true; - // isRotation = true; - // isScale = true; t.isTranslate = true; t.isRotation = true; - - std::cerr << "translate " << t.translate << '\n'; - std::cerr << "rotation_quad " << q << '\n'; - std::cerr << "scale " << t.scale << '\n'; } } return t;