diff --git a/examples/demo_runner.py b/examples/demo_runner.py index 42219c0cfe..9f3ea565a7 100644 --- a/examples/demo_runner.py +++ b/examples/demo_runner.py @@ -302,21 +302,21 @@ def do_time_steps(self): def print_semantic_scene(self): if self._sim_settings["print_semantic_scene"]: scene = self._sim.semantic_scene - print(f"House center:{scene.aabb.center} dims:{scene.aabb.sizes}") + print(f"House center:{scene.aabb.center} dims:{scene.aabb.size}") for level in scene.levels: print( f"Level id:{level.id}, center:{level.aabb.center}," - f" dims:{level.aabb.sizes}" + f" dims:{level.aabb.size}" ) for region in level.regions: print( f"Region id:{region.id}, category:{region.category.name()}," - f" center:{region.aabb.center}, dims:{region.aabb.sizes}" + f" center:{region.aabb.center}, dims:{region.aabb.size}" ) for obj in region.objects: print( f"Object id:{obj.id}, category:{obj.category.name()}," - f" center:{obj.aabb.center}, dims:{obj.aabb.sizes}" + f" center:{obj.aabb.center}, dims:{obj.aabb.size}" ) input("Press Enter to continue...") diff --git a/examples/tutorials/nb_python/ECCV_2020_Navigation.py b/examples/tutorials/nb_python/ECCV_2020_Navigation.py index 3f171de44c..47e9220e22 100644 --- a/examples/tutorials/nb_python/ECCV_2020_Navigation.py +++ b/examples/tutorials/nb_python/ECCV_2020_Navigation.py @@ -375,23 +375,23 @@ def print_scene_recur(scene, limit_output=10): print( f"House has {len(scene.levels)} levels, {len(scene.regions)} regions and {len(scene.objects)} objects" ) - print(f"House center:{scene.aabb.center} dims:{scene.aabb.sizes}") + print(f"House center:{scene.aabb.center} dims:{scene.aabb.size}") count = 0 for level in scene.levels: print( f"Level id:{level.id}, center:{level.aabb.center}," - f" dims:{level.aabb.sizes}" + f" dims:{level.aabb.size}" ) for region in level.regions: print( f"Region id:{region.id}, category:{region.category.name()}," - f" center:{region.aabb.center}, dims:{region.aabb.sizes}" + f" center:{region.aabb.center}, dims:{region.aabb.size}" ) for obj in region.objects: print( f"Object id:{obj.id}, category:{obj.category.name()}," - f" center:{obj.aabb.center}, dims:{obj.aabb.sizes}" + f" center:{obj.aabb.center}, dims:{obj.aabb.size}" ) count += 1 if count >= limit_output: diff --git a/examples/tutorials/new_actions.py b/examples/tutorials/new_actions.py index e58ca702fa..0b175db539 100644 --- a/examples/tutorials/new_actions.py +++ b/examples/tutorials/new_actions.py @@ -129,7 +129,9 @@ def _strafe_impl( np.array(scene_node.absolute_transformation().rotation_scaling()) @ habitat_sim.geo.FRONT ) - rotation = quat_from_angle_axis(np.deg2rad(strafe_angle), habitat_sim.geo.UP) + rotation = quat_from_angle_axis( + np.deg2rad(strafe_angle), np.array(habitat_sim.geo.UP) + ) move_ax = quat_rotate_vector(rotation, forward_ax) scene_node.translate_local(move_ax * forward_amount) diff --git a/examples/tutorials/notebooks/ECCV_2020_Navigation.ipynb b/examples/tutorials/notebooks/ECCV_2020_Navigation.ipynb index 36c7ed5e14..09c8f8bab2 100644 --- a/examples/tutorials/notebooks/ECCV_2020_Navigation.ipynb +++ b/examples/tutorials/notebooks/ECCV_2020_Navigation.ipynb @@ -450,23 +450,23 @@ " print(\n", " f\"House has {len(scene.levels)} levels, {len(scene.regions)} regions and {len(scene.objects)} objects\"\n", " )\n", - " print(f\"House center:{scene.aabb.center} dims:{scene.aabb.sizes}\")\n", + " print(f\"House center:{scene.aabb.center} dims:{scene.aabb.size}\")\n", "\n", " count = 0\n", " for level in scene.levels:\n", " print(\n", " f\"Level id:{level.id}, center:{level.aabb.center},\"\n", - " f\" dims:{level.aabb.sizes}\"\n", + " f\" dims:{level.aabb.size}\"\n", " )\n", " for region in level.regions:\n", " print(\n", " f\"Region id:{region.id}, category:{region.category.name()},\"\n", - " f\" center:{region.aabb.center}, dims:{region.aabb.sizes}\"\n", + " f\" center:{region.aabb.center}, dims:{region.aabb.size}\"\n", " )\n", " for obj in region.objects:\n", " print(\n", " f\"Object id:{obj.id}, category:{obj.category.name()},\"\n", - " f\" center:{obj.aabb.center}, dims:{obj.aabb.sizes}\"\n", + " f\" center:{obj.aabb.center}, dims:{obj.aabb.size}\"\n", " )\n", " count += 1\n", " if count >= limit_output:\n", diff --git a/src/esp/assets/BaseMesh.cpp b/src/esp/assets/BaseMesh.cpp index 5ccdf061dc..272634f894 100644 --- a/src/esp/assets/BaseMesh.cpp +++ b/src/esp/assets/BaseMesh.cpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include diff --git a/src/esp/assets/GenericSemanticMeshData.cpp b/src/esp/assets/GenericSemanticMeshData.cpp index 4c16184f83..23c98d5a68 100644 --- a/src/esp/assets/GenericSemanticMeshData.cpp +++ b/src/esp/assets/GenericSemanticMeshData.cpp @@ -19,6 +19,7 @@ #include #include +#include "esp/core/Utility.h" #include "esp/geo/Geo.h" #include "esp/scene/SemanticScene.h" @@ -65,8 +66,7 @@ GenericSemanticMeshData::buildSemanticMeshData( if (semanticFilename.find(".ply") != std::string::npos) { // Generic Semantic PLY meshes have -Z gravity const auto T_esp_scene = - Mn::Quaternion{quatf::FromTwoVectors(-vec3f::UnitZ(), geo::ESP_GRAVITY)} - .toMatrix(); + Mn::Quaternion::rotation(geo::ESP_FRONT, geo::ESP_GRAVITY).toMatrix(); for (auto& xyz : semanticMeshData->cpu_vbo_) { xyz = T_esp_scene * xyz; } @@ -307,7 +307,7 @@ GenericSemanticMeshData::buildSemanticMeshData( // display or save report denoting presence of semantic object-defined colors // in mesh return semanticMeshData; -} // GenericSemanticMeshData::buildSemanticMeshData +} // namespace assets std::vector> GenericSemanticMeshData::partitionSemanticMeshData( diff --git a/src/esp/assets/MeshData.h b/src/esp/assets/MeshData.h index 5cc8833a73..70affcd982 100644 --- a/src/esp/assets/MeshData.h +++ b/src/esp/assets/MeshData.h @@ -8,7 +8,6 @@ #include #include "esp/core/Esp.h" -#include "esp/core/EspEigen.h" namespace esp { namespace assets { @@ -16,13 +15,13 @@ namespace assets { //! Raw mesh data storage struct MeshData { //! Vertex positions - std::vector vbo; + std::vector vbo; //! Vertex normals - std::vector nbo; + std::vector nbo; //! Texture coordinates - std::vector tbo; + std::vector tbo; //! Vertex colors - std::vector cbo; + std::vector cbo; //! Index buffer std::vector ibo; diff --git a/src/esp/assets/MeshMetaData.h b/src/esp/assets/MeshMetaData.h index 130ac0e987..e54773ec52 100644 --- a/src/esp/assets/MeshMetaData.h +++ b/src/esp/assets/MeshMetaData.h @@ -140,9 +140,9 @@ struct MeshMetaData { * @param frame target frame in world space */ void setRootFrameOrientation(const geo::CoordinateFrame& frame) { - const quatf& transform = frame.rotationFrameToWorld(); - Magnum::Matrix4 R = Magnum::Matrix4::from( - Magnum::Quaternion(transform).toMatrix(), Magnum::Vector3()); + const Magnum::Quaternion& transform = frame.rotationFrameToWorld(); + Magnum::Matrix4 R = + Magnum::Matrix4::from(transform.toMatrix(), Magnum::Vector3()); root.transformFromLocalToParent = R * root.transformFromLocalToParent; } }; diff --git a/src/esp/assets/ResourceManager.cpp b/src/esp/assets/ResourceManager.cpp index 521ca1a65a..8301d846c4 100644 --- a/src/esp/assets/ResourceManager.cpp +++ b/src/esp/assets/ResourceManager.cpp @@ -20,8 +20,6 @@ #include #include #include -#include -#include #include #include #include @@ -694,11 +692,8 @@ esp::geo::CoordinateFrame ResourceManager::buildFrameFromAttributes( const Mn::Vector3& up, const Mn::Vector3& front, const Mn::Vector3& origin) { - const vec3f upEigen{Mn::EigenIntegration::cast(up)}; - const vec3f frontEigen{Mn::EigenIntegration::cast(front)}; - if (upEigen.isOrthogonal(frontEigen)) { - const vec3f originEigen{Mn::EigenIntegration::cast(origin)}; - esp::geo::CoordinateFrame frame{upEigen, frontEigen, originEigen}; + if (abs(Mn::Math::dot(up, front)) < Mn::Math::TypeTraits::epsilon()) { + esp::geo::CoordinateFrame frame{up, front, origin}; return frame; } ESP_DEBUG(Mn::Debug::Flag::NoSpace) @@ -3481,8 +3476,7 @@ void ResourceManager::joinHierarchy( << "` so skipping join."; } else { for (const auto& pos : meshData.positions) { - mesh.vbo.push_back(Mn::EigenIntegration::cast( - transformFromLocalToWorld.transformPoint(pos))); + mesh.vbo.push_back(transformFromLocalToWorld.transformPoint(pos)); } for (const auto& index : meshData.indices) { mesh.ibo.push_back(index + lastIndex); @@ -3529,8 +3523,7 @@ void ResourceManager::joinSemanticHierarchy( // Save the vertices for (const auto& pos : vertices) { - mesh.vbo.push_back(Mn::EigenIntegration::cast( - transformFromLocalToWorld.transformPoint(pos))); + mesh.vbo.push_back(transformFromLocalToWorld.transformPoint(pos)); } // Save the indices diff --git a/src/esp/bindings/Bindings.cpp b/src/esp/bindings/Bindings.cpp index 18a9482ef6..88e7ada293 100644 --- a/src/esp/bindings/Bindings.cpp +++ b/src/esp/bindings/Bindings.cpp @@ -14,17 +14,6 @@ namespace py = pybind11; using py::literals::operator""_a; -namespace esp { - -void initEspBindings(py::module& m) { - // ==== box3f ==== - py::class_(m, "BBox") - .def_property_readonly("sizes", &box3f::sizes) - .def_property_readonly("center", &box3f::center); -} - -} // namespace esp - PYBIND11_MODULE(habitat_sim_bindings, m) { m.attr("cuda_enabled") = #ifdef ESP_BUILD_WITH_CUDA @@ -59,7 +48,6 @@ PYBIND11_MODULE(habitat_sim_bindings, m) { py::bind_map>(m, "MapStringString"); - esp::initEspBindings(m); esp::core::config::initConfigBindings(m); esp::core::initCoreBindings(m); esp::geo::initGeoBindings(m); diff --git a/src/esp/bindings/GeoBindings.cpp b/src/esp/bindings/GeoBindings.cpp index 13a867bf3f..41701ead55 100644 --- a/src/esp/bindings/GeoBindings.cpp +++ b/src/esp/bindings/GeoBindings.cpp @@ -7,8 +7,6 @@ #include "esp/geo/Geo.h" #include "esp/geo/OBB.h" -#include - namespace Mn = Magnum; namespace py = pybind11; using py::literals::operator""_a; @@ -23,18 +21,17 @@ void initGeoBindings(py::module& m) { geo.attr("GRAVITY") = ESP_GRAVITY; geo.attr("FRONT") = ESP_FRONT; geo.attr("BACK") = ESP_BACK; - geo.attr("LEFT") = ESP_FRONT.cross(ESP_GRAVITY); - geo.attr("RIGHT") = ESP_FRONT.cross(ESP_UP); + geo.attr("LEFT") = Mn::Math::cross(ESP_FRONT, ESP_GRAVITY); + geo.attr("RIGHT") = Mn::Math::cross(ESP_FRONT, ESP_UP); // ==== OBB ==== py::class_(m, "OBB", R"(This is an OBB.)") - .def(py::init([](const vec3f& center, const vec3f& dimensions, + .def(py::init([](const Mn::Vector3& center, const Mn::Vector3& dimensions, const Mn::Quaternion& rotation) { - return OBB(center, dimensions, - Mn::EigenIntegration::cast(rotation)); + return OBB(center, dimensions, rotation); }), "center"_a, "dimensions"_a, "rotation"_a) - .def(py::init()) + .def(py::init()) .def( "contains", &OBB::contains, R"(Returns whether world coordinate point p is contained in this OBB within threshold distance epsilon.)") @@ -48,7 +45,7 @@ void initGeoBindings(py::module& m) { .def( "rotate", [](OBB& self, const Mn::Quaternion& rotation) { - return self.rotate(Mn::EigenIntegration::cast(rotation)); + return self.rotate(rotation); }, R"(Rotate this OBB by the given rotation and return reference to self.)") .def_property_readonly("center", &OBB::center, R"(Centroid of this OBB.)") @@ -59,15 +56,19 @@ void initGeoBindings(py::module& m) { .def_property_readonly("half_extents", &OBB::halfExtents, R"(Half-extents of this OBB (dimensions).)") .def_property_readonly( - "rotation", [](const OBB& self) { return self.rotation().coeffs(); }, + // Return as an array of format [x,y,z,w] to retain existing + // expectations + "rotation", + [](const OBB& self) { + const Mn::Quaternion q = self.rotation(); + return Mn::Vector4(q.vector(), q.scalar()); + }, R"(Quaternion representing rotation of this OBB.)") .def_property_readonly( - "local_to_world", - [](const OBB& self) { return self.localToWorld().matrix(); }, + "local_to_world", [](const OBB& self) { return self.localToWorld(); }, R"(Transform from local [0,1]^3 coordinates to world coordinates.)") .def_property_readonly( - "world_to_local", - [](const OBB& self) { return self.worldToLocal().matrix(); }, + "world_to_local", [](const OBB& self) { return self.worldToLocal(); }, R"(Transform from world coordinates to local [0,1]^3 coordinates.)"); geo.def( diff --git a/src/esp/bindings/SceneBindings.cpp b/src/esp/bindings/SceneBindings.cpp index 0e43a47e19..cbc18f0f17 100644 --- a/src/esp/bindings/SceneBindings.cpp +++ b/src/esp/bindings/SceneBindings.cpp @@ -253,11 +253,11 @@ void initSceneBindings( .def_static( "load_mp3d_house", [](const std::string& filename, SemanticScene& scene, - const vec4f& rotation) { + const Mn::Vector4& rotation) { // numpy doesn't have a quaternion equivalent, use vec4 // instead - return SemanticScene::loadMp3dHouse( - filename, scene, Eigen::Map(rotation.data())); + return SemanticScene::loadMp3dHouse(filename, scene, + {rotation.xyz(), rotation.w()}); }, R"( Loads a SemanticScene from a Matterport3D House format file into passed diff --git a/src/esp/bindings/ShortestPathBindings.cpp b/src/esp/bindings/ShortestPathBindings.cpp index a1f4d27dad..39f96735cc 100644 --- a/src/esp/bindings/ShortestPathBindings.cpp +++ b/src/esp/bindings/ShortestPathBindings.cpp @@ -174,24 +174,25 @@ void initShortestPathBindings(py::module& m) { R"(Finds the shortest path between a start point and the closest of a set of end points (in geodesic distance) on the navigation mesh using MultiGoalShortestPath module. Path variable is filled if successful. Returns boolean success.)") .def("try_step", &PathFinder::tryStep, "start"_a, "end"_a) - .def("try_step", &PathFinder::tryStep, "start"_a, "end"_a) + .def("try_step", &PathFinder::tryStep, "start"_a, + "end"_a) + .def("try_step_no_sliding", + &PathFinder::tryStepNoSliding, "start"_a, "end"_a) .def("try_step_no_sliding", &PathFinder::tryStepNoSliding, "start"_a, "end"_a) - .def("try_step_no_sliding", &PathFinder::tryStepNoSliding, - "start"_a, "end"_a) .def("snap_point", &PathFinder::snapPoint, "point"_a, "island_index"_a = ID_UNDEFINED) - .def("snap_point", &PathFinder::snapPoint, "point"_a, + .def("snap_point", &PathFinder::snapPoint, "point"_a, "island_index"_a = ID_UNDEFINED) .def( "get_island", &PathFinder::getIsland, "point"_a, R"(Query the island closest to a point. Snaps the point to the NavMesh first, so check the snap distance also if unsure.)") .def( - "get_island", &PathFinder::getIsland, "point"_a, + "get_island", &PathFinder::getIsland, "point"_a, R"(Query the island closest to a point. Snaps the point to the NavMesh first, so check the snap distance also if unsure.)") .def( "island_radius", - [](PathFinder& self, const vec3f& pt) { + [](PathFinder& self, const Magnum::Vector3& pt) { return self.islandRadius(pt); }, "pt"_a, diff --git a/src/esp/core/EspEigen.h b/src/esp/core/EspEigen.h index 56b86c4f24..4562d9f03f 100644 --- a/src/esp/core/EspEigen.h +++ b/src/esp/core/EspEigen.h @@ -13,131 +13,11 @@ #endif #include #include -// #include - -#include -#include -#include -#include - -#include "esp/core/configure.h" namespace Eigen { -typedef Matrix Vector3uc; -typedef Matrix Vector3ui; -typedef Matrix Vector4uc; -typedef Matrix Vector4ui; -typedef Matrix Vector4ul; typedef Matrix RowMatrixXf; -//! Eigen JSON string format specification -static const IOFormat kJsonFormat(StreamPrecision, - DontAlignCols, - ",", // coef separator - ",", // row separator - "", // row prefix - "", // col prefix - "[", // mat prefix - "]"); // mat suffix - -template -std::ostream& operator<<(std::ostream& os, - const Matrix& matrix) { - return os << matrix.format(kJsonFormat); -} - -//! Write Eigen matrix types into ostream in JSON string format -template -typename std::enable_if::type -operator<<(Corrade::Utility::Debug& os, - const Matrix& matrix) { - return os << matrix.format(kJsonFormat); -} - -template -typename std::enable_if<(numRows != Dynamic && numCols != Dynamic) && - (numRows != 1 && numCols != 1), - Corrade::Utility::Debug&>::type -operator<<(Corrade::Utility::Debug& os, - const Matrix& matrix) { - return os << Magnum::Math::RectangularMatrix{matrix}; -} - -template -typename std::enable_if<(numRows != Dynamic && numCols != Dynamic) && - (numRows == 1 || numCols == 1), - Corrade::Utility::Debug&>::type -operator<<(Corrade::Utility::Debug& os, - const Matrix& matrix) { - return os << Magnum::Math::Vector<(numRows == 1 ? numCols : numRows), T>{ - matrix}; -} - -//! Write Eigen map into ostream in JSON string format -template -std::ostream& operator<<(std::ostream& os, const Map& m) { - return os << m.format(kJsonFormat); -} - } // namespace Eigen -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2f) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3f) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4f) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4d) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2i) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3i) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4i) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3f) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4f) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4d) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4uc) - -//! core simulator namespace -namespace esp { - -// basic types -typedef Eigen::Vector2f vec2f; -typedef Eigen::Vector3f vec3f; -typedef Eigen::Vector4f vec4f; -typedef Eigen::Vector2d vec2d; -typedef Eigen::Vector3d vec3d; -typedef Eigen::Vector4d vec4d; -typedef Eigen::Vector2i vec2i; -typedef Eigen::Vector3i vec3i; -typedef Eigen::Vector4i vec4i; -typedef Eigen::Matrix3f mat3f; -typedef Eigen::Matrix4f mat4f; -typedef Eigen::Matrix3d mat3d; -typedef Eigen::Matrix4d mat4d; -typedef Eigen::Quaternionf quatf; -typedef Eigen::Vector3uc vec3uc; -typedef Eigen::Vector3ui vec3ui; -typedef Eigen::Vector4uc vec4uc; -typedef Eigen::Vector4ui vec4ui; -typedef Eigen::Vector4i vec4i; -typedef Eigen::Vector4ul vec4ul; -typedef Eigen::VectorXi vecXi; -typedef Eigen::AlignedBox3f box3f; - -typedef Eigen::Transform Transform; - -//! Write box3f into ostream in JSON string format -inline std::ostream& operator<<(std::ostream& os, const box3f& bbox) { - return os << "{min:" << bbox.min() << ",max:" << bbox.max() << "}"; -} - -//! Write box3f as a magnum range -inline Corrade::Utility::Debug& operator<<(Corrade::Utility::Debug& os, - const box3f& bbox) { - return os << Magnum::Range3D{bbox}; -} - -} // namespace esp - #endif // ESP_CORE_ESPEIGEN_H_ diff --git a/src/esp/core/Utility.h b/src/esp/core/Utility.h index 6b60596ba2..b960393612 100644 --- a/src/esp/core/Utility.h +++ b/src/esp/core/Utility.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include #include @@ -37,6 +39,36 @@ inline Magnum::Quaternion randomRotation() { return Magnum::Quaternion(qAxis, sqrt(1 - u1) * sin(2 * M_PI * u2)); } +/** + * @brief Build a Magnum Quaternion as a rotation from two vectors + * @param rotFrom The vector to rotate. + * @param rotTo The vector to rotate to. + * @return normalized rotation quaternion to perform the rotation. + */ +template +Magnum::Math::Quaternion quatRotFromTwoVectors( + const Magnum::Math::Vector3& rotFrom, + const Magnum::Math::Vector3& rotTo) { + const auto fromNorm = rotFrom.normalized(); + const auto toNorm = rotTo.normalized(); + + if (fromNorm == -toNorm) { + // colinear opposite direction + // Find a vector not colinear with rotFrom + auto axisVec = Magnum::Math::Vector3::xAxis(); + if (Magnum::Math::abs(Magnum::Math::dot(fromNorm, axisVec)) == 1.0f) { + axisVec = Magnum::Math::Vector3::yAxis(); + } + // Find a normal vector ortho to a and b, treat as rotational axis + const auto rotAxisVec = Magnum::Math::cross(fromNorm, axisVec).normalized(); + return Magnum::Math::Quaternion(rotAxisVec, 0).normalized(); + } + const auto halfVec = (fromNorm + toNorm).normalized(); + return Magnum::Math::Quaternion(Magnum::Math::cross(fromNorm, halfVec), + Magnum::Math::dot(fromNorm, halfVec)) + .normalized(); +} // quatRotFromTwoVectors + template Magnum::Math::Matrix4 orthonormalizeRotationShear( const Magnum::Math::Matrix4& transformation) { diff --git a/src/esp/geo/CoordinateFrame.cpp b/src/esp/geo/CoordinateFrame.cpp index bd04b12862..ed21a138f6 100644 --- a/src/esp/geo/CoordinateFrame.cpp +++ b/src/esp/geo/CoordinateFrame.cpp @@ -3,43 +3,54 @@ // LICENSE file in the root directory of this source tree. #include "CoordinateFrame.h" +#include +#include +#include "esp/core/Utility.h" #include "esp/geo/Geo.h" namespace esp { namespace geo { -CoordinateFrame::CoordinateFrame(const vec3f& up /* = ESP_UP */, - const vec3f& front /* = ESP_FRONT */, - const vec3f& origin /* = vec3f(0, 0, 0) */) +namespace Mn = Magnum; +CoordinateFrame::CoordinateFrame( + const Mn::Vector3& up /* = ESP_UP */, + const Mn::Vector3& front /* = ESP_FRONT */, + const Mn::Vector3& origin /* = Mn::Vector3(0, 0, 0) */) : up_(up), front_(front), origin_(origin) { - CORRADE_INTERNAL_ASSERT(up_.isOrthogonal(front_)); + CORRADE_INTERNAL_ASSERT(std::abs(Mn::Math::dot(up_, front_)) < + Mn::Math::TypeTraits::epsilon()); } -CoordinateFrame::CoordinateFrame(const quatf& rotation, - const vec3f& origin /* = vec3f(0, 0, 0) */) - : CoordinateFrame(rotation * ESP_UP, rotation * ESP_FRONT, origin) {} - -quatf CoordinateFrame::rotationWorldToFrame() const { - const quatf R_frameUp_worldUp = quatf::FromTwoVectors(ESP_UP, up_); - return quatf::FromTwoVectors(R_frameUp_worldUp * ESP_FRONT, front_) * - R_frameUp_worldUp; +CoordinateFrame::CoordinateFrame( + const Mn::Quaternion& rotation, + const Mn::Vector3& origin /* = Mn::Vector3(0, 0, 0) */) + : CoordinateFrame(rotation.transformVectorNormalized(ESP_UP), + rotation.transformVectorNormalized(ESP_FRONT), + origin) {} + +Mn::Quaternion CoordinateFrame::rotationWorldToFrame() const { + const Mn::Quaternion R_frameUp_worldUp = + Mn::Quaternion::rotation(ESP_UP, up_); + return (Mn::Quaternion::rotation( + R_frameUp_worldUp.transformVectorNormalized(ESP_FRONT), front_) * + R_frameUp_worldUp) + .normalized(); } -quatf CoordinateFrame::rotationFrameToWorld() const { - return rotationWorldToFrame().inverse(); +Mn::Quaternion CoordinateFrame::rotationFrameToWorld() const { + return rotationWorldToFrame().invertedNormalized(); } std::string CoordinateFrame::toString() const { - std::stringstream ss; - ss << "{\"up\":" << up() << ",\"front\":" << front() - << ",\"origin\":" << origin() << "}"; - return ss.str(); + return Cr::Utility::formatString( + "\"up\":[{},{},{}],\"front\":[{},{},{}],\"origin\":[{},{},{}]", up().x(), + up().y(), up().z(), front().x(), front().y(), front().z(), origin().x(), + origin().y(), origin().z()); } bool operator==(const CoordinateFrame& a, const CoordinateFrame& b) { - return a.up().isApprox(b.up()) && a.front().isApprox(b.front()) && - a.origin().isApprox(b.origin()); + return a.up() == b.up() && a.front() == b.front() && a.origin() == b.origin(); } bool operator!=(const CoordinateFrame& a, const CoordinateFrame& b) { return !(a == b); diff --git a/src/esp/geo/CoordinateFrame.h b/src/esp/geo/CoordinateFrame.h index 375447b44e..ca1c12a167 100644 --- a/src/esp/geo/CoordinateFrame.h +++ b/src/esp/geo/CoordinateFrame.h @@ -16,45 +16,42 @@ namespace geo { //! equivalently "gravity" and "back" class CoordinateFrame { public: - explicit CoordinateFrame(const vec3f& up = ESP_UP, - const vec3f& front = ESP_FRONT, - const vec3f& origin = vec3f::Zero()); - explicit CoordinateFrame(const quatf& rotation, - const vec3f& origin = vec3f::Zero()); + explicit CoordinateFrame(const Magnum::Vector3& up = ESP_UP, + const Magnum::Vector3& front = ESP_FRONT, + const Magnum::Vector3& origin = {}); + explicit CoordinateFrame(const Magnum::Quaternion& rotation, + const Magnum::Vector3& origin = {}); //! Returns position of origin of this CoordinateFrame relative to parent - vec3f origin() const { return origin_; } + Magnum::Vector3 origin() const { return origin_; } //! Returns up orientation - vec3f up() const { return up_; } + Magnum::Vector3 up() const { return up_; } //! Returns down/gravity orientation - vec3f gravity() const { return -up_; } + Magnum::Vector3 gravity() const { return -up_; } //! Returns front orientation - vec3f front() const { return front_; } + Magnum::Vector3 front() const { return front_; } //! Returns front orientation - vec3f back() const { return -front_; } + Magnum::Vector3 back() const { return -front_; } //! Returns quaternion representing the rotation taking direction vectors in //! world coordinates to direction vectors in this CoordinateFrame - quatf rotationWorldToFrame() const; + Magnum::Quaternion rotationWorldToFrame() const; //! Returns quaternion representing the rotation taking direction vectors in //! this CoordinateFrame to direction vectors in world coordinates - quatf rotationFrameToWorld() const; - - //! Return Transform from world coordinates to local coordinates - Transform transformationWorldToFrame() const; + Magnum::Quaternion rotationFrameToWorld() const; //! Returns a stringified JSON representation of this CoordinateFrame std::string toString() const; protected: - vec3f up_; - vec3f front_; - vec3f origin_; + Magnum::Vector3 up_; + Magnum::Vector3 front_; + Magnum::Vector3 origin_; ESP_SMART_POINTERS(CoordinateFrame) }; diff --git a/src/esp/geo/Geo.cpp b/src/esp/geo/Geo.cpp index 59e554f7ca..2f3daf518e 100644 --- a/src/esp/geo/Geo.cpp +++ b/src/esp/geo/Geo.cpp @@ -7,8 +7,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -18,11 +20,12 @@ using Magnum::Math::Literals::operator""_rgb; namespace esp { namespace geo { -std::vector convexHull2D(const std::vector& points) { +std::vector convexHull2D(const std::vector& points) { CORRADE_INTERNAL_ASSERT(points.size() > 2); - auto cross = [](const vec2f& o, const vec2f& a, const vec2f& b) { - return (a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0)); + auto cross = [](const Mn::Vector2& o, const Mn::Vector2& a, + const Mn::Vector2& b) { + return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0]); }; // Sort indices of points lexicographically @@ -30,8 +33,8 @@ std::vector convexHull2D(const std::vector& points) { std::iota(idx.begin(), idx.end(), 0); std::sort( idx.begin(), idx.end(), [&points](const size_t& a, const size_t& b) { - return points[a](0) < points[b](0) || - (points[a](0) == points[b](0) && points[a](1) < points[b](1)); + return points[a][0] < points[b][0] || + (points[a][0] == points[b][0] && points[a][1] < points[b][1]); }); std::vector hullIdx(2 * idx.size()); @@ -58,7 +61,7 @@ std::vector convexHull2D(const std::vector& points) { hullIdx.resize(k - 1); - std::vector hull; + std::vector hull; hull.reserve(hullIdx.size()); for (auto& ix : hullIdx) { @@ -79,13 +82,13 @@ std::vector convexHull2D(const std::vector& points) { * where x_0, x are the coordinates before and after transformation, t is the * translation. * - * x = R * (c0 + y0) + t = (Rc0 + t) + Ry0 eq(1) + * x = R * (c0 + y0) + t = (Rc0 + t) + Ry0 eq[1] * * Our Goal is to find the x_max and x_min after the transformation. * * First, determining the x_max: * - * In eq(1), Rc0 + t is a constant, which means max{x} iff max{Ry0} + * In eq[1], Rc0 + t is a constant, which means max{x} iff max{Ry0} * * R looks like: * [R0, R1, R2] diff --git a/src/esp/geo/Geo.h b/src/esp/geo/Geo.h index d3c2c58896..2655851bba 100644 --- a/src/esp/geo/Geo.h +++ b/src/esp/geo/Geo.h @@ -11,7 +11,6 @@ #include #include "esp/core/Esp.h" -#include "esp/core/EspEigen.h" #include #include @@ -46,16 +45,16 @@ enum class ColorSpace { }; //! global/world up direction -static const vec3f ESP_UP = vec3f::UnitY(); +static const Mn::Vector3 ESP_UP = Mn::Vector3::yAxis(); //! global/world gravity (down) direction -static const vec3f ESP_GRAVITY = -ESP_UP; +static const Mn::Vector3 ESP_GRAVITY = -ESP_UP; //! global/world front direction -static const vec3f ESP_FRONT = -vec3f::UnitZ(); +static const Mn::Vector3 ESP_FRONT = -Mn::Vector3::zAxis(); //! global/world back direction -static const vec3f ESP_BACK = -ESP_FRONT; +static const Mn::Vector3 ESP_BACK = -ESP_FRONT; // compute convex hull of 2D points and return as vector of vertices -std::vector convexHull2D(const std::vector& points); +std::vector convexHull2D(const std::vector& points); /** * @brief Compute the axis-aligned bounding box which results from applying a diff --git a/src/esp/geo/OBB.cpp b/src/esp/geo/OBB.cpp index a917a6fa8b..59c9249c05 100644 --- a/src/esp/geo/OBB.cpp +++ b/src/esp/geo/OBB.cpp @@ -4,69 +4,70 @@ #include "OBB.h" +#include +#include +#include +#include + #include #include #include "esp/core/Check.h" +#include "esp/core/Utility.h" #include "esp/geo/Geo.h" +namespace Mn = Magnum; namespace esp { namespace geo { -OBB::OBB() { - center_.setZero(); - halfExtents_.setZero(); - rotation_.setIdentity(); -} - -OBB::OBB(const vec3f& center, const vec3f& dimensions, const quatf& rotation) +OBB::OBB(const Mn::Vector3& center, + const Mn::Vector3& dimensions, + const Mn::Quaternion& rotation) : center_(center), halfExtents_(dimensions * 0.5), rotation_(rotation) { recomputeTransforms(); } -OBB::OBB(const box3f& aabb) - : OBB(aabb.center(), aabb.sizes(), quatf::Identity()) {} +OBB::OBB(const Mn::Range3D& aabb) + : OBB(aabb.center(), aabb.size(), Mn::Quaternion(Mn::Math::IdentityInit)) {} -static const vec3f kCorners[8] = { - vec3f(-1, -1, -1), vec3f(-1, -1, +1), vec3f(-1, +1, -1), vec3f(-1, +1, +1), - vec3f(+1, -1, -1), vec3f(+1, -1, +1), vec3f(+1, +1, -1), vec3f(+1, +1, +1)}; +static const Mn::Vector3 kCorners[8] = { + Mn::Vector3(-1, -1, -1), Mn::Vector3(-1, -1, +1), Mn::Vector3(-1, +1, -1), + Mn::Vector3(-1, +1, +1), Mn::Vector3(+1, -1, -1), Mn::Vector3(+1, -1, +1), + Mn::Vector3(+1, +1, -1), Mn::Vector3(+1, +1, +1)}; -box3f OBB::toAABB() const { - box3f bbox; +Mn::Range3D OBB::toAABB() const { + Mn::Range3D bbox; for (int i = 0; i < 8; ++i) { - const vec3f worldPoint = - center_ + (rotation_ * kCorners[i].cwiseProduct(halfExtents_)); - bbox.extend(worldPoint); + const Mn::Vector3 worldPoint = + center_ + + (rotation_.transformVectorNormalized(kCorners[i] * halfExtents_)); + bbox = Mn::Math::join(bbox, worldPoint); } return bbox; } void OBB::recomputeTransforms() { - ESP_CHECK(center_.allFinite(), - "Illegal center for OBB. Cannot recompute transformations."); - ESP_CHECK(halfExtents_.allFinite(), - "Illegal size values for OBB. Cannot recompute transformations."); - ESP_CHECK( - rotation_.coeffs().allFinite(), - "Illegal rotation quaternion for OBB. Cannot recompute transformations."); - - // TODO(MS): these can be composed more efficiently and directly - const mat3f R = rotation_.matrix(); + const Mn::Matrix3 R = rotation_.toMatrix(); // Local-to-world transform + Mn::Matrix3 localToWorldRot; for (int i = 0; i < 3; ++i) { - localToWorld_.linear().col(i) = R.col(i) * halfExtents_[i]; + localToWorldRot[i] = R[i] * halfExtents_[i]; } - localToWorld_.translation() = center_; + + localToWorld_ = Mn::Matrix4::from(localToWorldRot, center_); // World-to-local transform. Points within OBB are in [0,1]^3 + Mn::Matrix3 worldToLocalRotTranspose; for (int i = 0; i < 3; ++i) { - worldToLocal_.linear().row(i) = R.col(i) * (1.0f / halfExtents_[i]); + worldToLocalRotTranspose[i] = R[i] * (1.0f / halfExtents_[i]); } - worldToLocal_.translation() = -worldToLocal_.linear() * center_; + worldToLocal_ = + Mn::Matrix4::from(worldToLocalRotTranspose.transposed(), + (-worldToLocalRotTranspose.transposed() * center_)); } -bool OBB::contains(const vec3f& p, float eps /* = 1e-6f */) const { - const vec3f pLocal = worldToLocal() * p; +bool OBB::contains(const Mn::Vector3& p, float eps /* = 1e-6f */) const { + const Mn::Vector3 pLocal = worldToLocal().transformPoint(p); const float bound = 1.0f + eps; for (int i = 0; i < 3; ++i) { if (std::abs(pLocal[i]) > bound) { @@ -76,39 +77,43 @@ bool OBB::contains(const vec3f& p, float eps /* = 1e-6f */) const { return true; // Here only if all three coords within bounds } -float OBB::distance(const vec3f& p) const { +float OBB::distance(const Mn::Vector3& p) const { if (contains(p)) { return 0; } - const vec3f closest = closestPoint(p); - return (p - closest).norm(); + const Mn::Vector3 closest = closestPoint(p); + return (p - closest).length(); } -vec3f OBB::closestPoint(const vec3f& p) const { - const vec3f d = p - center_; - vec3f closest = center_; - const mat3f R = rotation_.matrix(); +Mn::Vector3 OBB::closestPoint(const Mn::Vector3& p) const { + const Mn::Vector3 d = p - center_; + Mn::Vector3 closest = center_; + const auto R = rotation_.toMatrix(); for (int i = 0; i < 3; ++i) { closest += - clamp(R.col(i).dot(d), -halfExtents_[i], halfExtents_[i]) * R.col(i); + clamp(Mn::Math::dot(R[i], d), -halfExtents_[i], halfExtents_[i]) * R[i]; } return closest; } -OBB& OBB::rotate(const quatf& q) { +OBB& OBB::rotate(const Mn::Quaternion& q) { rotation_ = q * rotation_; recomputeTransforms(); return *this; } // https://geidav.wordpress.com/tag/minimum-obb/ -OBB computeGravityAlignedMOBB(const vec3f& gravity, - const std::vector& points) { - const auto align_gravity = quatf::FromTwoVectors(gravity, -vec3f::UnitZ()); +OBB computeGravityAlignedMOBB(const Mn::Vector3& gravity, + const std::vector& points) { + const auto align_gravity = + Mn::Quaternion::rotation(gravity, -Mn::Vector3::zAxis()); - static auto ortho = [](const vec2f& v) { return vec2f(v[1], -v[0]); }; - static auto intersect_lines = [](const vec2f& s0, const vec2f& d0, - const vec2f& s1, const vec2f& d1) { + static auto ortho = [](const Mn::Vector2& v) { + return Mn::Vector2(v[1], -v[0]); + }; + static auto intersect_lines = [](const Mn::Vector2& s0, const Mn::Vector2& d0, + const Mn::Vector2& s1, + const Mn::Vector2& d1) { const float dd = d0[0] * d1[1] - d0[1] * d1[0]; const float dx = s1[0] - s0[0]; @@ -117,40 +122,40 @@ OBB computeGravityAlignedMOBB(const vec3f& gravity, return s0 + t * d0; }; - static auto mobb_area = [](const vec2f& left_start, const vec2f& left_dir, - const vec2f& right_start, const vec2f& right_dir, - const vec2f& top_start, const vec2f& top_dir, - const vec2f& bottom_start, - const vec2f& bottom_dir) { - const vec2f upper_left = - intersect_lines(left_start, left_dir, top_start, top_dir); - const vec2f upper_right = - intersect_lines(right_start, right_dir, top_start, top_dir); - const vec2f bottom_left = - intersect_lines(bottom_start, bottom_dir, left_start, left_dir); - - return (upper_left - upper_right).norm() * - (upper_left - bottom_left).norm(); - }; - - std::vector in_plane_points; + static auto mobb_area = + [](const Mn::Vector2& left_start, const Mn::Vector2& left_dir, + const Mn::Vector2& right_start, const Mn::Vector2& right_dir, + const Mn::Vector2& top_start, const Mn::Vector2& top_dir, + const Mn::Vector2& bottom_start, const Mn::Vector2& bottom_dir) { + const Mn::Vector2 upper_left = + intersect_lines(left_start, left_dir, top_start, top_dir); + const Mn::Vector2 upper_right = + intersect_lines(right_start, right_dir, top_start, top_dir); + const Mn::Vector2 bottom_left = + intersect_lines(bottom_start, bottom_dir, left_start, left_dir); + + return (upper_left - upper_right).length() * + (upper_left - bottom_left).length(); + }; + + std::vector in_plane_points; in_plane_points.reserve(points.size()); for (const auto& pt : points) { - vec3f aligned_pt = align_gravity * pt; + Mn::Vector3 aligned_pt = align_gravity.transformVectorNormalized(pt); in_plane_points.emplace_back(aligned_pt[0], aligned_pt[1]); } const auto hull = convexHull2D(in_plane_points); CORRADE_INTERNAL_ASSERT(hull.size() > 0); - std::vector edge_dirs; + std::vector edge_dirs; edge_dirs.reserve(hull.size()); for (size_t i = 0; i < hull.size(); ++i) { edge_dirs.emplace_back( (hull[(i + 1) % hull.size()] - hull[i]).normalized()); } - vec2f min_pt = hull[0], max_pt = hull[0]; + Mn::Vector2 min_pt = hull[0], max_pt = hull[0]; int left_idx = 0, right_idx = 0, top_idx = 0, bottom_idx = 0; for (size_t i = 0; i < hull.size(); ++i) { const auto& pt = hull[i]; @@ -175,17 +180,17 @@ OBB computeGravityAlignedMOBB(const vec3f& gravity, } } - vec2f left_dir = vec2f(0, -1), right_dir = vec2f(0, 1), - top_dir = vec2f(-1, 0), bottom_dir = vec2f(1, 0); + Mn::Vector2 left_dir = Mn::Vector2(0, -1), right_dir = Mn::Vector2(0, 1), + top_dir = Mn::Vector2(-1, 0), bottom_dir = Mn::Vector2(1, 0); float best_area = 1e10; - vec2f best_bottom_dir = vec2f(NAN, NAN); + Mn::Vector2 best_bottom_dir = Mn::Vector2(NAN, NAN); for (size_t i = 0; i < hull.size(); ++i) { const std::array angles( - {std::acos(left_dir.dot(edge_dirs[left_idx])), - std::acos(right_dir.dot(edge_dirs[right_idx])), - std::acos(top_dir.dot(edge_dirs[top_idx])), - std::acos(bottom_dir.dot(edge_dirs[bottom_idx]))}); + {std::acos(Mn::Math::dot(left_dir, edge_dirs[left_idx])), + std::acos(Mn::Math::dot(right_dir, edge_dirs[right_idx])), + std::acos(Mn::Math::dot(top_dir, edge_dirs[top_idx])), + std::acos(Mn::Math::dot(bottom_dir, edge_dirs[bottom_idx]))}); float min_angle = 1e10; size_t best_line = 0; for (size_t i = 0; i < angles.size(); ++i) { @@ -236,18 +241,21 @@ OBB computeGravityAlignedMOBB(const vec3f& gravity, best_area = area; } } - const auto T_w2b = - quatf::FromTwoVectors(vec3f(best_bottom_dir[0], best_bottom_dir[1], 0), - vec3f::UnitX()) * - align_gravity; + const auto T_w2b = Mn::Quaternion::rotation( + Mn::Vector3(best_bottom_dir[0], best_bottom_dir[1], 0), + Mn::Vector3::xAxis()) * + align_gravity; - box3f aabb; - aabb.setEmpty(); + Mn::Range3D aabb; for (const auto& pt : points) { - aabb.extend(T_w2b * pt); + const auto transPt = T_w2b.transformVectorNormalized(pt); + aabb = Mn::Math::join(aabb, pt); } + // Inverted normalized rotation + const auto inverseT_w2b = T_w2b.inverted().normalized(); - return OBB{T_w2b.inverse() * aabb.center(), aabb.sizes(), T_w2b.inverse()}; + return OBB{inverseT_w2b.transformVectorNormalized(aabb.center()), aabb.size(), + inverseT_w2b}; } } // namespace geo diff --git a/src/esp/geo/OBB.h b/src/esp/geo/OBB.h index 3ab77f7817..a56ef5d364 100644 --- a/src/esp/geo/OBB.h +++ b/src/esp/geo/OBB.h @@ -5,6 +5,10 @@ #ifndef ESP_GEO_OBB_H_ #define ESP_GEO_OBB_H_ +#include +#include +#include +#include #include "esp/core/Esp.h" #include "esp/geo/Geo.h" @@ -14,17 +18,17 @@ namespace geo { // oriented bounding box class OBB { public: - explicit OBB(); - explicit OBB(const vec3f& center, - const vec3f& dimensions, - const quatf& rotation); - explicit OBB(const box3f& aabb); + explicit OBB() = default; + explicit OBB(const Mn::Vector3& center, + const Mn::Vector3& dimensions, + const Mn::Quaternion& rotation); + explicit OBB(const Mn::Range3D& aabb); //! Returns centroid of this OBB - vec3f center() const { return center_; } + Mn::Vector3 center() const { return center_; } //! Returns the dimensions of this OBB in its own frame of reference - vec3f sizes() const { return halfExtents_ * 2; } + Mn::Vector3 sizes() const { return halfExtents_ * 2; } /** * @brief Return the volume of this bbox @@ -34,53 +38,61 @@ class OBB { } //! Returns half-extents of this OBB (dimensions) - vec3f halfExtents() const { return halfExtents_; } + Mn::Vector3 halfExtents() const { return halfExtents_; } //! Returns quaternion representing rotation of this OBB - quatf rotation() const { return rotation_; } + Mn::Quaternion rotation() const { return rotation_; } - //! Return Transform from world coordinates to local [0,1]^3 coordinates - const Transform& worldToLocal() const { return worldToLocal_; } + //! Return Mn::Matrix4 from world coordinates to local [0,1]^3 coordinates + const Mn::Matrix4& worldToLocal() const { return worldToLocal_; } - //! Return Transform from local [0,1]^3 coordinates to world coordinates - const Transform& localToWorld() const { return localToWorld_; } + //! Return Mn::Matrix4 from local [0,1]^3 coordinates to world coordinates + const Mn::Matrix4& localToWorld() const { return localToWorld_; } //! Returns an axis aligned bounding box bounding this OBB - box3f toAABB() const; + Mn::Range3D toAABB() const; //! Returns distance to p from closest point on OBB surface //! (0 if point p is inside box) - float distance(const vec3f& p) const; + float distance(const Mn::Vector3& p) const; //! Return closest point to p within OBB. If p is inside return p. - vec3f closestPoint(const vec3f& p) const; + Mn::Vector3 closestPoint(const Mn::Vector3& p) const; //! Returns whether world coordinate point p is contained in this OBB within //! threshold distance epsilon - bool contains(const vec3f& p, float epsilon = 1e-6f) const; + bool contains(const Mn::Vector3& p, float epsilon = 1e-6f) const; //! Rotate this OBB by the given rotation and return reference to self - OBB& rotate(const quatf& rotation); + OBB& rotate(const Mn::Quaternion& rotation); protected: void recomputeTransforms(); - vec3f center_; - vec3f halfExtents_; - quatf rotation_; - Transform localToWorld_, worldToLocal_; + Mn::Vector3 center_; + Mn::Vector3 halfExtents_; + Mn::Quaternion rotation_; + Mn::Matrix4 localToWorld_; + Mn::Matrix4 worldToLocal_; ESP_SMART_POINTERS(OBB) }; inline std::ostream& operator<<(std::ostream& os, const OBB& obb) { - return os << "{c:" << obb.center() << ",h:" << obb.halfExtents() - << ",r:" << obb.rotation().coeffs() << "}"; + auto rotQuat = obb.rotation(); + float scalar = rotQuat.scalar(); + Mn::Vector3 v = rotQuat.vector(); + Mn::Vector3 c = obb.center(); + Mn::Vector3 h = obb.halfExtents(); + + return os << Cr::Utility::formatString( + "ctr : [{} {} {}], h : [{} {} {}], rot : [{} {} {}], {}", c.x(), + c.y(), c.z(), h.x(), h.y(), h.z(), v.x(), v.y(), v.z(), scalar); } // compute a minimum area OBB containing given points, and constrained to // have -Z axis along given gravity orientation -OBB computeGravityAlignedMOBB(const vec3f& gravity, - const std::vector& points); +OBB computeGravityAlignedMOBB(const Mn::Vector3& gravity, + const std::vector& points); } // namespace geo } // namespace esp diff --git a/src/esp/gfx/BackgroundRenderer.h b/src/esp/gfx/BackgroundRenderer.h index fc59e4140a..4b8d015f51 100644 --- a/src/esp/gfx/BackgroundRenderer.h +++ b/src/esp/gfx/BackgroundRenderer.h @@ -9,6 +9,7 @@ #ifdef ESP_BUILD_WITH_BACKGROUND_RENDERER +#include #include #include "esp/gfx/RenderCamera.h" diff --git a/src/esp/gfx/CubeMapCamera.cpp b/src/esp/gfx/CubeMapCamera.cpp index 449ffe4aa8..5a4b7aeec1 100644 --- a/src/esp/gfx/CubeMapCamera.cpp +++ b/src/esp/gfx/CubeMapCamera.cpp @@ -3,8 +3,6 @@ // LICENSE file in the root directory of this source tree #include "CubeMapCamera.h" -#include - namespace Mn = Magnum; namespace Cr = Corrade; @@ -17,17 +15,7 @@ CubeMapCamera::CubeMapCamera( : RenderCamera(node, semanticDataIDX) { updateOriginalViewingMatrix(); } -CubeMapCamera::CubeMapCamera( - scene::SceneNode& node, - esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, - const vec3f& eye, - const vec3f& target, - const vec3f& up) - : CubeMapCamera(node, - semanticDataIDX, - Mn::Vector3{eye}, - Mn::Vector3{target}, - Mn::Vector3{up}) {} + CubeMapCamera::CubeMapCamera( scene::SceneNode& node, esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, diff --git a/src/esp/gfx/CubeMapCamera.h b/src/esp/gfx/CubeMapCamera.h index 588115015b..2cb129146e 100644 --- a/src/esp/gfx/CubeMapCamera.h +++ b/src/esp/gfx/CubeMapCamera.h @@ -28,18 +28,6 @@ class CubeMapCamera : public RenderCamera { * @param target the target position (parent node space) * @param up the up direction (parent node space) */ - explicit CubeMapCamera(scene::SceneNode& node, - esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, - const vec3f& eye, - const vec3f& target, - const vec3f& up); - /** - * @brief Constructor - * @param node the scene node to which the camera is attached - * @param eye the eye position (parent node space) - * @param target the target position (parent node space) - * @param up the up direction (parent node space) - */ explicit CubeMapCamera(scene::SceneNode& node, esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, const Magnum::Vector3& eye, diff --git a/src/esp/gfx/RenderCamera.cpp b/src/esp/gfx/RenderCamera.cpp index 605bfcbe8e..fe0a2a8e78 100644 --- a/src/esp/gfx/RenderCamera.cpp +++ b/src/esp/gfx/RenderCamera.cpp @@ -4,11 +4,11 @@ #include "RenderCamera.h" -#include #include #include #include #include +#include #include "esp/scene/SceneGraph.h" namespace Mn = Magnum; @@ -67,17 +67,6 @@ RenderCamera::RenderCamera(scene::SceneNode& node, resetViewingParameters(eye, target, up); } -RenderCamera::RenderCamera(scene::SceneNode& node, - esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, - const vec3f& eye, - const vec3f& target, - const vec3f& up) - : RenderCamera(node, - semanticDataIDX, - Mn::Vector3{eye}, - Mn::Vector3{target}, - Mn::Vector3{up}) {} - RenderCamera& RenderCamera::resetViewingParameters(const Mn::Vector3& eye, const Mn::Vector3& target, const Mn::Vector3& up) { diff --git a/src/esp/gfx/RenderCamera.h b/src/esp/gfx/RenderCamera.h index 4201a9ba3b..a01ee6935f 100644 --- a/src/esp/gfx/RenderCamera.h +++ b/src/esp/gfx/RenderCamera.h @@ -81,21 +81,6 @@ class RenderCamera : public MagnumCamera { * @param up the up direction (in PARENT node space) * NOTE: it will override any relative transformation w.r.t its parent node */ - RenderCamera(scene::SceneNode& node, - esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, - const vec3f& eye, - const vec3f& target, - const vec3f& up); - /** - * @brief Constructor - * @param node the scene node to which the camera is attached - * @param semanticDataIDX The type of semantic id data rendered by this - * camera. Ignored if not rendering semantic data. - * @param eye the eye position (in PARENT node space) - * @param target the target position (in PARENT node space) - * @param up the up direction (in PARENT node space) - * NOTE: it will override any relative transformation w.r.t its parent node - */ RenderCamera(scene::SceneNode& node, esp::scene::SceneNodeSemanticDataIDX semanticDataIDX, const Magnum::Vector3& eye, diff --git a/src/esp/gfx/RenderTarget.cpp b/src/esp/gfx/RenderTarget.cpp index 8adff08d89..8dd15415d3 100644 --- a/src/esp/gfx/RenderTarget.cpp +++ b/src/esp/gfx/RenderTarget.cpp @@ -2,6 +2,7 @@ // This source code is licensed under the MIT license found in the // LICENSE file in the root directory of this source tree. +#include #include #include #include diff --git a/src/esp/io/Json.cpp b/src/esp/io/Json.cpp index 4660912fb6..173dd8b1b5 100644 --- a/src/esp/io/Json.cpp +++ b/src/esp/io/Json.cpp @@ -96,15 +96,5 @@ std::string jsonToString(const JsonDocument& d, int maxDecimalPlaces) { return buffer.GetString(); } -vec3f jsonToVec3f(const JsonGenericValue& jsonArray) { - vec3f vec; - size_t dim = 0; - CORRADE_INTERNAL_ASSERT(jsonArray.GetArray().Size() == vec.size()); - for (const auto& element : jsonArray.GetArray()) { - vec[dim++] = element.GetFloat(); - } - return vec; -} - } // namespace io } // namespace esp diff --git a/src/esp/io/Json.h b/src/esp/io/Json.h index a3e49984ce..763805513b 100644 --- a/src/esp/io/Json.h +++ b/src/esp/io/Json.h @@ -50,9 +50,6 @@ JsonDocument parseJsonString(const std::string& jsonString); //! Return string representation of given JsonDocument std::string jsonToString(const JsonDocument& d, int maxDecimalPlaces = -1); -//! Return Vec3f coordinates representation of given JsonObject of array type -esp::vec3f jsonToVec3f(const JsonGenericValue& jsonArray); - /** * @brief Check passed json doc for existence of passed jsonTag as value of * type T. If present, populate passed setter with value. Returns diff --git a/src/esp/io/JsonEspTypes.h b/src/esp/io/JsonEspTypes.h index 3a854bfc39..5e4a182ce4 100644 --- a/src/esp/io/JsonEspTypes.h +++ b/src/esp/io/JsonEspTypes.h @@ -20,27 +20,6 @@ namespace esp { namespace io { -inline JsonGenericValue toJsonValue(const esp::vec3f& vec, - JsonAllocator& allocator) { - return toJsonArrayHelper(vec.data(), 3, allocator); -} - -inline bool fromJsonValue(const JsonGenericValue& obj, esp::vec3f& val) { - if (obj.IsArray() && obj.Size() == 3) { - for (rapidjson::SizeType i = 0; i < 3; ++i) { - if (obj[i].IsNumber()) { - val[i] = obj[i].GetDouble(); - } else { - ESP_ERROR() << "Invalid numeric value specified in JSON vec3f, index :" - << i; - return false; - } - } - return true; - } - return false; -} - inline JsonGenericValue toJsonValue( const esp::assets::PhongMaterialColor& material, JsonAllocator& allocator) { @@ -72,9 +51,9 @@ inline JsonGenericValue toJsonValue(const esp::geo::CoordinateFrame& frame, inline bool fromJsonValue(const JsonGenericValue& obj, esp::geo::CoordinateFrame& frame) { bool success = true; - esp::vec3f up; - esp::vec3f front; - esp::vec3f origin; + Mn::Vector3 up; + Mn::Vector3 front; + Mn::Vector3 origin; success &= readMember(obj, "up", up); success &= readMember(obj, "front", front); success &= readMember(obj, "origin", origin); diff --git a/src/esp/nav/GreedyFollower.cpp b/src/esp/nav/GreedyFollower.cpp index de5714635c..b9d2e08fbd 100644 --- a/src/esp/nav/GreedyFollower.cpp +++ b/src/esp/nav/GreedyFollower.cpp @@ -5,14 +5,10 @@ #include "esp/nav/GreedyFollower.h" -#include -#include - #include "esp/core/Esp.h" #include "esp/geo/Geo.h" namespace Mn = Magnum; -using Mn::EigenIntegration::cast; namespace esp { namespace nav { @@ -22,9 +18,9 @@ GreedyGeodesicFollowerImpl::GreedyGeodesicFollowerImpl( MoveFn& moveForward, MoveFn& turnLeft, MoveFn& turnRight, - double goalDist, - double forwardAmount, - double turnAmount, + float goalDist, + float forwardAmount, + float turnAmount, bool fixThrashing, int thrashingThreshold) : pathfinder_{pathfinder}, @@ -35,12 +31,12 @@ GreedyGeodesicFollowerImpl::GreedyGeodesicFollowerImpl( goalDist_{goalDist}, turnAmount_{turnAmount}, fixThrashing_{fixThrashing}, - thrashingThreshold_{thrashingThreshold} {}; + thrashingThreshold_{thrashingThreshold} {} float GreedyGeodesicFollowerImpl::geoDist(const Mn::Vector3& start, const Mn::Vector3& end) { - geoDistPath_.requestedStart = cast(start); - geoDistPath_.requestedEnd = cast(end); + geoDistPath_.requestedStart = start; + geoDistPath_.requestedEnd = end; pathfinder_->findPath(geoDistPath_); return geoDistPath_.geodesicDistance; } @@ -56,7 +52,7 @@ GreedyGeodesicFollowerImpl::TryStepResult GreedyGeodesicFollowerImpl::tryStep( const float geoDistAfter = geoDist(newPose, end); const float distToObsAfter = pathfinder_->distanceToClosestObstacle( - cast(newPose), 1.1 * closeToObsThreshold_); + newPose, 1.1f * closeToObsThreshold_); return {geoDistAfter, distToObsAfter, didCollide}; } @@ -106,7 +102,7 @@ GreedyGeodesicFollowerImpl::nextBestPrimAlong(const core::RigidState& state, // Plan over all primitives of the form [LEFT] * n + [FORWARD] // or [RIGHT] * n + [FORWARD] - for (float angle = 0; angle < M_PI; angle += turnAmount_) { + for (float angle = 0; angle < Mn::Constants::pi(); angle += turnAmount_) { { const float reward = computeReward(leftDummyNode_, path, leftPrim.size()); if (reward > bestReward) { @@ -163,8 +159,8 @@ GreedyGeodesicFollowerImpl::CODES GreedyGeodesicFollowerImpl::nextActionAlong( const core::RigidState& start, const Mn::Vector3& end) { ShortestPath path; - path.requestedStart = cast(start.translation); - path.requestedEnd = cast(end); + path.requestedStart = start.translation; + path.requestedEnd = end; pathfinder_->findPath(path); CODES nextAction; @@ -200,8 +196,8 @@ GreedyGeodesicFollowerImpl::findPath(const core::RigidState& start, core::RigidState state{findPathDummyNode_.rotation(), findPathDummyNode_.MagnumObject::translation()}; ShortestPath path; - path.requestedStart = cast(state.translation); - path.requestedEnd = cast(end); + path.requestedStart = state.translation; + path.requestedEnd = end; pathfinder_->findPath(path); const auto nextPrim = nextBestPrimAlong(state, path); if (nextPrim.empty()) { diff --git a/src/esp/nav/GreedyFollower.h b/src/esp/nav/GreedyFollower.h index 4640ebb1d7..ccf0bf718a 100644 --- a/src/esp/nav/GreedyFollower.h +++ b/src/esp/nav/GreedyFollower.h @@ -75,9 +75,9 @@ class GreedyGeodesicFollowerImpl { MoveFn& moveForward, MoveFn& turnLeft, MoveFn& turnRight, - double goalDist, - double forwardAmount, - double turnAmount, + float goalDist, + float forwardAmount, + float turnAmount, bool fixThrashing = true, int thrashingThreshold = 16); @@ -123,7 +123,7 @@ class GreedyGeodesicFollowerImpl { private: PathFinder::ptr pathfinder_; MoveFn moveForward_, turnLeft_, turnRight_; - const double forwardAmount_, goalDist_, turnAmount_; + const float forwardAmount_, goalDist_, turnAmount_; const bool fixThrashing_; const int thrashingThreshold_; const float closeToObsThreshold_ = 0.2f; diff --git a/src/esp/nav/PathFinder.cpp b/src/esp/nav/PathFinder.cpp index 89bcb5865a..61581e1b3c 100644 --- a/src/esp/nav/PathFinder.cpp +++ b/src/esp/nav/PathFinder.cpp @@ -3,21 +3,21 @@ // LICENSE file in the root directory of this source tree. #include "PathFinder.h" +#include #include #include #include #include #include +#include #include -#include -#include - #include #include #include + // NOLINTNEXTLINE #define _USE_MATH_DEFINES #include @@ -26,6 +26,7 @@ #include "esp/assets/MeshData.h" #include "esp/core/Esp.h" +#include "esp/core/EspEigen.h" #include "DetourCommon.h" #include "DetourNavMesh.h" @@ -46,7 +47,7 @@ namespace esp { namespace nav { bool operator==(const NavMeshSettings& a, const NavMeshSettings& b) { -#define CLOSE(name) (std::abs(a.name - b.name) < 1e-5) +#define CLOSE(name) (std::abs(a.name - b.name) < 1e-5f) #define EQ(name) (a.name == b.name) return CLOSE(cellSize) && CLOSE(cellHeight) && CLOSE(agentHeight) && @@ -93,23 +94,23 @@ void NavMeshSettings::writeToJSON(const std::string& jsonFile) const { } struct MultiGoalShortestPath::Impl { - std::vector requestedEnds; + std::vector requestedEnds; std::vector endRefs; //! Tracks whether an endpoint is valid or not as determined by setup to avoid //! extra work or issues later. std::vector endIsValid; - std::vector pathEnds; + std::vector pathEnds; std::vector minTheoreticalDist; - vec3f prevRequestedStart = vec3f::Zero(); + Mn::Vector3 prevRequestedStart; }; MultiGoalShortestPath::MultiGoalShortestPath() - : pimpl_{spimpl::make_unique_impl()} {}; + : pimpl_{spimpl::make_unique_impl()} {} void MultiGoalShortestPath::setRequestedEnds( - const std::vector& newEnds) { + const std::vector& newEnds) { pimpl_->endRefs.clear(); pimpl_->pathEnds.clear(); pimpl_->requestedEnds = newEnds; @@ -117,13 +118,14 @@ void MultiGoalShortestPath::setRequestedEnds( pimpl_->minTheoreticalDist.assign(newEnds.size(), 0); } -const std::vector& MultiGoalShortestPath::getRequestedEnds() const { +const std::vector& MultiGoalShortestPath::getRequestedEnds() + const { return pimpl_->requestedEnds; } namespace { template -std::tuple projectToPoly( +std::tuple projectToPoly( const T& pt, const dtNavMeshQuery* navQuery, const dtQueryFilter* filter) { @@ -134,7 +136,7 @@ std::tuple projectToPoly( dtPolyRef polyRef = 0; // Initialize with all NANs at dtStatusSucceed(status) == true does NOT mean // that it found a point to project to.......... - vec3f polyXYZ = vec3f::Constant(Mn::Constants::nan()); + Mn::Vector3 polyXYZ = Mn::Vector3(Mn::Constants::nan()); dtStatus status = navQuery->findNearestPoly(pt.data(), polyPickExt, filter, &polyRef, polyXYZ.data()); @@ -166,7 +168,7 @@ inline ushort orFlag(ushort curFlags, ushort flag) { class IslandSystem { public: IslandSystem(const dtNavMesh* navMesh, const dtQueryFilter* filter) { - std::vector islandVerts; + std::vector islandVerts; // Iterate over all tiles for (int iTile = 0; iTile < navMesh->getMaxTiles(); ++iTile) { @@ -188,7 +190,7 @@ class IslandSystem { // The radius is calculated as the max deviation from the mean for all // points in the island - vec3f centroid = vec3f::Zero(); + Mn::Vector3 centroid; for (auto& v : islandVerts) { centroid += v; } @@ -196,7 +198,7 @@ class IslandSystem { float maxRadius = 0.0; for (auto& v : islandVerts) { - maxRadius = std::max(maxRadius, (v - centroid).norm()); + maxRadius = std::max(maxRadius, (v - centroid).length()); } islandRadius_.emplace_back(maxRadius); @@ -327,7 +329,7 @@ class IslandSystem { */ inline void setPolyFlagForIslandCircle(dtNavMesh* navMesh, ushort flag, - const vec3f& circleCenter, + const Mn::Vector3& circleCenter, const float radius, int islandIndex = ID_UNDEFINED) { assertValidIsland(islandIndex); @@ -409,7 +411,7 @@ class IslandSystem { const dtQueryFilter* filter, const uint32_t newIslandId, const dtPolyRef& startRef, - std::vector& islandVerts) { + std::vector& islandVerts) { islandsToPolys_[newIslandId].push_back(startRef); polyToIsland_.emplace(startRef, newIslandId); islandVerts.clear(); @@ -429,7 +431,7 @@ class IslandSystem { navMesh->getTileAndPolyByRefUnsafe(ref, &tile, &poly); for (int iVert = 0; iVert < poly->vertCount; ++iVert) { - islandVerts.emplace_back(Eigen::Map( + islandVerts.emplace_back(Mn::Vector3::from( &tile->verts[static_cast(poly->verts[iVert]) * 3])); } @@ -472,16 +474,18 @@ struct PathFinder::Impl { const float* bmax); bool build(const NavMeshSettings& bs, const esp::assets::MeshData& mesh); - vec3f getRandomNavigablePoint(int maxTries, - int islandIndex /*= ID_UNDEFINED*/); - vec3f getRandomNavigablePointAroundSphere(const vec3f& circleCenter, - float radius, - int maxTries, - int islandIndex /*= ID_UNDEFINED*/); - vec3f getRandomNavigablePointInCircle(const vec3f& circleCenter, - float radius, - int maxTries, - int islandIndex /*= ID_UNDEFINED*/); + Mn::Vector3 getRandomNavigablePoint(int maxTries, + int islandIndex /*= ID_UNDEFINED*/); + Mn::Vector3 getRandomNavigablePointAroundSphere( + const Mn::Vector3& circleCenter, + float radius, + int maxTries, + int islandIndex /*= ID_UNDEFINED*/); + Mn::Vector3 getRandomNavigablePointInCircle( + const Mn::Vector3& circleCenter, + float radius, + int maxTries, + int islandIndex /*= ID_UNDEFINED*/); bool findPath(ShortestPath& path); bool findPath(MultiGoalShortestPath& path); @@ -509,24 +513,24 @@ struct PathFinder::Impl { void seed(uint32_t newSeed); - float islandRadius(const vec3f& pt) const; + float islandRadius(const Mn::Vector3& pt) const; float islandRadius(int islandIndex) const; - float distanceToClosestObstacle(const vec3f& pt, - float maxSearchRadius = 2.0) const; - HitRecord closestObstacleSurfacePoint(const vec3f& pt, - float maxSearchRadius = 2.0) const; + float distanceToClosestObstacle(const Mn::Vector3& pt, + float maxSearchRadius = 2.0f) const; + HitRecord closestObstacleSurfacePoint(const Mn::Vector3& pt, + float maxSearchRadius = 2.0f) const; - bool isNavigable(const vec3f& pt, float maxYDelta = 0.5) const; + bool isNavigable(const Mn::Vector3& pt, float maxYDelta = 0.5f) const; - std::pair bounds() const { return bounds_; }; + std::pair bounds() const { return bounds_; }; - Eigen::Matrix - getTopDownView(float metersPerPixel, float height, float eps) const; + MatrixXb getTopDownView(float metersPerPixel, float height, float eps) const; - Eigen::Matrix - getTopDownIslandView(float metersPerPixel, float height, float eps) const; + MatrixXi getTopDownIslandView(float metersPerPixel, + float height, + float eps) const; assets::MeshData::ptr getNavMeshData(int islandIndex /*= ID_UNDEFINED*/); @@ -552,21 +556,21 @@ struct PathFinder::Impl { std::unordered_map islandMeshData_; Cr::Containers::Optional navMeshSettings_; - std::pair bounds_; + std::pair bounds_; bool initNavQuery(); - Cr::Containers::Optional>> - findPathInternal(const vec3f& start, + Cr::Containers::Optional>> + findPathInternal(const Mn::Vector3& start, dtPolyRef startRef, - const vec3f& pathStart, - const vec3f& end, + const Mn::Vector3& pathStart, + const Mn::Vector3& end, dtPolyRef endRef, - const vec3f& pathEnd); + const Mn::Vector3& pathEnd); bool findPathSetup(MultiGoalShortestPath& path, dtPolyRef& startRef, - vec3f& pathStart); + Mn::Vector3& pathStart); }; namespace { @@ -918,7 +922,7 @@ bool PathFinder::Impl::build(const NavMeshSettings& bs, return false; } - bounds_ = std::make_pair(vec3f(bmin), vec3f(bmax)); + bounds_ = std::make_pair(Mn::Vector3::from(bmin), Mn::Vector3::from(bmax)); ESP_DEBUG() << "Created navmesh with" << ws.pmesh->nverts << "vertices" << ws.pmesh->npolys << "polygons"; @@ -951,13 +955,13 @@ bool PathFinder::Impl::build(const NavMeshSettings& bs, const int numVerts = mesh.vbo.size(); const int numIndices = mesh.ibo.size(); const float mf = std::numeric_limits::max(); - vec3f bmin(mf, mf, mf); - vec3f bmax(-mf, -mf, -mf); + Mn::Vector3 bmin(mf, mf, mf); + Mn::Vector3 bmax(-mf, -mf, -mf); for (int i = 0; i < numVerts; ++i) { - const vec3f& p = mesh.vbo[i]; - bmin = bmin.cwiseMin(p); - bmax = bmax.cwiseMax(p); + const Mn::Vector3& p = mesh.vbo[i]; + bmin = Mn::Math::min(bmin, p); + bmax = Mn::Math::max(bmax, p); } int* indices = new int[numIndices]; @@ -988,7 +992,7 @@ struct NavMeshTileHeader { }; struct Triangle { - std::vector v; + std::vector v; Triangle() { v.resize(3); } }; @@ -1003,16 +1007,16 @@ std::vector getPolygonTriangles(const dtPoly* poly, for (int j = 0; j < pd->triCount; ++j) { const unsigned char* t = &tile->detailTris[static_cast((pd->triBase + j)) * 4]; - const float* v[3]; + // const float* v[3]; for (int k = 0; k < 3; ++k) { if (t[k] < poly->vertCount) - triangles[j].v[k] = Eigen::Map( - &tile->verts[static_cast(poly->verts[t[k]]) * 3]); + triangles[j].v[k] = Mn::Vector3::from( + (&tile->verts[static_cast(poly->verts[t[k]]) * 3])); else - triangles[j].v[k] = Eigen::Map( - &tile->detailVerts[static_cast( - (pd->vertBase + (t[k] - poly->vertCount))) * - 3]); + triangles[j].v[k] = Mn::Vector3::from( + (&tile->detailVerts[static_cast( + (pd->vertBase + (t[k] - poly->vertCount))) * + 3])); } } @@ -1026,9 +1030,9 @@ float polyArea(const dtPoly* poly, const dtMeshTile* tile) { float area = 0; for (auto& tri : triangles) { - const vec3f w1 = tri.v[1] - tri.v[0]; - const vec3f w2 = tri.v[2] - tri.v[1]; - area += 0.5 * w1.cross(w2).norm(); + const Mn::Vector3 w1 = tri.v[1] - tri.v[0]; + const Mn::Vector3 w2 = tri.v[2] - tri.v[1]; + area += 0.5f * Mn::Math::cross(w1, w2).length(); } return area; @@ -1065,7 +1069,7 @@ void impl::IslandSystem::removeZeroAreaPolys(dtNavMesh* navMesh) { CORRADE_INTERNAL_ASSERT(tmp != nullptr); float polygonArea = polyArea(poly, tile); - if (polygonArea < 1e-5) { + if (polygonArea < 1e-5f) { navMesh->setPolyFlags(polyRef, POLYFLAGS_DISABLED); } else if ((poly->flags & POLYFLAGS_WALK) != 0) { islandsToArea_[polyToIsland_[polyRef]] += polygonArea; @@ -1114,7 +1118,7 @@ bool PathFinder::Impl::loadNavMesh(const std::string& path) { << "NavMeshSettings aren't present, guessing that they are the default"; } - vec3f bmin, bmax; + Mn::Vector3 bmin, bmax; dtNavMesh* mesh = dtAllocNavMesh(); if (!mesh) { @@ -1155,11 +1159,11 @@ bool PathFinder::Impl::loadNavMesh(const std::string& path) { tileHeader.tileRef, nullptr); const dtMeshTile* tile = mesh->getTileByRef(tileHeader.tileRef); if (i == 0) { - bmin = vec3f(tile->header->bmin); - bmax = vec3f(tile->header->bmax); + bmin = Mn::Vector3::from(tile->header->bmin); + bmax = Mn::Vector3::from(tile->header->bmax); } else { - bmin = bmin.array().min(Eigen::Array3f{tile->header->bmin}); - bmax = bmax.array().max(Eigen::Array3f{tile->header->bmax}); + bmin = Mn::Math::min(bmin, Mn::Vector3::from(tile->header->bmin)); + bmax = Mn::Math::max(bmax, Mn::Vector3::from(tile->header->bmax)); } } @@ -1230,11 +1234,11 @@ static float frand() { return static_cast(rand()) / static_cast(RAND_MAX); } -vec3f PathFinder::Impl::getRandomNavigablePoint( +Mn::Vector3 PathFinder::Impl::getRandomNavigablePoint( const int maxTries /*= 10*/, int islandIndex /*= ID_UNDEFINED*/) { islandSystem_->assertValidIsland(islandIndex); - if (getNavigableArea(islandIndex) <= 0.0) + if (getNavigableArea(islandIndex) <= 0.0f) throw std::runtime_error( "NavMesh has no navigable area, this indicates an issue with the " "NavMesh"); @@ -1249,7 +1253,7 @@ vec3f PathFinder::Impl::getRandomNavigablePoint( PolyFlags::POLYFLAGS_OFF_ISLAND); } - vec3f pt; + Mn::Vector3 pt; int i = 0; for (i = 0; i < maxTries; ++i) { dtPolyRef ref = 0; @@ -1272,20 +1276,20 @@ vec3f PathFinder::Impl::getRandomNavigablePoint( if (i == maxTries) { ESP_ERROR() << "Failed to getRandomNavigablePoint. Try increasing max " "tries if the navmesh is fine but just hard to sample from"; - return vec3f::Constant(Mn::Constants::nan()); + return Mn::Vector3(Mn::Constants::nan()); } return pt; } -vec3f PathFinder::Impl::getRandomNavigablePointInCircle( - const vec3f& circleCenter, +Mn::Vector3 PathFinder::Impl::getRandomNavigablePointInCircle( + const Mn::Vector3& circleCenter, const float radius, const int maxTries, int islandIndex) { float radSqr = radius * radius; islandSystem_->assertValidIsland(islandIndex); - if (getNavigableArea(islandIndex) <= 0.0) + if (getNavigableArea(islandIndex) <= 0.0f) throw std::runtime_error( "NavMesh has no navigable area, this indicates an issue with the " "NavMesh"); @@ -1297,7 +1301,7 @@ vec3f PathFinder::Impl::getRandomNavigablePointInCircle( filter_->setExcludeFlags(filter_->getExcludeFlags() | PolyFlags::POLYFLAGS_OFF_ISLAND); - vec3f pt; + Mn::Vector3 pt; int i = 0; for (i = 0; i < maxTries; ++i) { dtPolyRef ref = 0; @@ -1323,18 +1327,18 @@ vec3f PathFinder::Impl::getRandomNavigablePointInCircle( if (i == maxTries) { ESP_ERROR() << "Failed to getRandomNavigablePoint. Try increasing max " "tries if the navmesh is fine but just hard to sample from"; - return vec3f::Constant(Mn::Constants::nan()); + return Mn::Vector3(Mn::Constants::nan()); } return pt; } -vec3f PathFinder::Impl::getRandomNavigablePointAroundSphere( - const vec3f& circleCenter, +Mn::Vector3 PathFinder::Impl::getRandomNavigablePointAroundSphere( + const Mn::Vector3& circleCenter, const float radius, const int maxTries, int islandIndex) { islandSystem_->assertValidIsland(islandIndex); - if (getNavigableArea(islandIndex) <= 0.0) + if (getNavigableArea(islandIndex) <= 0.0f) throw std::runtime_error( "NavMesh has no navigable area, this indicates an issue with the " "NavMesh"); @@ -1349,11 +1353,11 @@ vec3f PathFinder::Impl::getRandomNavigablePointAroundSphere( PolyFlags::POLYFLAGS_OFF_ISLAND); } - vec3f pt = vec3f::Constant(Mn::Constants::nan()); + Mn::Vector3 pt = Mn::Vector3(Mn::Constants::nan()); dtPolyRef start_ref = 0; // ID to start our search dtStatus status = navQuery_->findNearestPoly( - circleCenter.data(), vec3f{radius, radius, radius}.data(), filter_.get(), - &start_ref, pt.data()); + circleCenter.data(), Mn::Vector3{radius, radius, radius}.data(), + filter_.get(), &start_ref, pt.data()); // cache and handle later to unify required clean-up bool failedAndAborting = (!dtStatusSucceed(status) || std::isnan(pt[0])); @@ -1365,7 +1369,8 @@ vec3f PathFinder::Impl::getRandomNavigablePointAroundSphere( status = navQuery_->findRandomPointAroundCircle( start_ref, circleCenter.data(), radius, filter_.get(), frand, &rand_ref, pt.data()); - if (dtStatusSucceed(status) && (pt - circleCenter).norm() <= radius) { + if (dtStatusSucceed(status) && + (pt - circleCenter).dot() <= radius * radius) { break; } } @@ -1382,24 +1387,24 @@ vec3f PathFinder::Impl::getRandomNavigablePointAroundSphere( if (failedAndAborting) { ESP_ERROR() << "Failed to getRandomNavigablePoint. No polygon found within radius"; - return vec3f::Constant(Mn::Constants::nan()); + return Mn::Vector3(Mn::Constants::nan()); } if (i == maxTries) { ESP_ERROR() << "Failed to getRandomNavigablePoint. Try increasing max " "tries if the navmesh is fine but just hard to sample from"; - return vec3f::Constant(Mn::Constants::nan()); + return Mn::Vector3(Mn::Constants::nan()); } return pt; } namespace { -float pathLength(const std::vector& points) { +float pathLength(const std::vector& points) { CORRADE_INTERNAL_ASSERT(points.size() > 0); float length = 0; - const vec3f* previousPoint = &points[0]; + const Mn::Vector3* previousPoint = &points[0]; for (const auto& pt : points) { - length += (*previousPoint - pt).norm(); + length += (*previousPoint - pt).length(); previousPoint = &pt; } @@ -1419,16 +1424,16 @@ bool PathFinder::Impl::findPath(ShortestPath& path) { return status; } -Cr::Containers::Optional>> -PathFinder::Impl::findPathInternal(const vec3f& start, +Cr::Containers::Optional>> +PathFinder::Impl::findPathInternal(const Mn::Vector3& start, dtPolyRef startRef, - const vec3f& pathStart, - const vec3f& end, + const Mn::Vector3& pathStart, + const Mn::Vector3& end, dtPolyRef endRef, - const vec3f& pathEnd) { + const Mn::Vector3& pathEnd) { // check if trivial path (start is same as end) and early return - if (pathStart.isApprox(pathEnd)) { - return std::make_tuple(0.0f, std::vector{pathStart, pathEnd}); + if (pathStart == pathEnd) { + return std::make_tuple(0.0f, std::vector{pathStart, pathEnd}); } // Check if there is a path between the start and any of the ends @@ -1448,7 +1453,7 @@ PathFinder::Impl::findPathInternal(const vec3f& start, } int numPoints = 0; - std::vector points(MAX_POLYS); + std::vector points(MAX_POLYS); status = navQuery_->findStraightPath(start.data(), end.data(), polys, numPolys, points[0].data(), nullptr, nullptr, &numPoints, MAX_POLYS); @@ -1465,7 +1470,7 @@ PathFinder::Impl::findPathInternal(const vec3f& start, bool PathFinder::Impl::findPathSetup(MultiGoalShortestPath& path, dtPolyRef& startRef, - vec3f& pathStart) { + Mn::Vector3& pathStart) { path.geodesicDistance = std::numeric_limits::infinity(); path.closestEndPointIndex = -1; path.points.clear(); @@ -1485,7 +1490,7 @@ bool PathFinder::Impl::findPathSetup(MultiGoalShortestPath& path, int numValidPoints = 0; for (const auto& rqEnd : path.getRequestedEnds()) { dtPolyRef endRef = 0; - vec3f pathEnd; + Mn::Vector3 pathEnd; std::tie(status, endRef, pathEnd) = projectToPoly(rqEnd, navQuery_.get(), filter_.get()); @@ -1510,7 +1515,7 @@ bool PathFinder::Impl::findPathSetup(MultiGoalShortestPath& path, bool PathFinder::Impl::findPath(MultiGoalShortestPath& path) { dtPolyRef startRef = 0; - vec3f pathStart; + Mn::Vector3 pathStart; if (!findPathSetup(path, startRef, pathStart)) return false; @@ -1525,10 +1530,10 @@ bool PathFinder::Impl::findPath(MultiGoalShortestPath& path) { findPath(prevPath); const float movedAmount = prevPath.geodesicDistance; - for (int i = 0; i < path.pimpl_->requestedEnds.size(); ++i) { + for (std::size_t i = 0; i < path.pimpl_->requestedEnds.size(); ++i) { path.pimpl_->minTheoreticalDist[i] = std::max( path.pimpl_->minTheoreticalDist[i] - movedAmount, - (path.pimpl_->requestedEnds[i] - path.requestedStart).norm()); + (path.pimpl_->requestedEnds[i] - path.requestedStart).length()); } path.pimpl_->prevRequestedStart = path.requestedStart; @@ -1550,7 +1555,7 @@ bool PathFinder::Impl::findPath(MultiGoalShortestPath& path) { if (path.pimpl_->minTheoreticalDist[i] > path.geodesicDistance) continue; - const Cr::Containers::Optional>> + const Cr::Containers::Optional>> findResult = findPathInternal(path.requestedStart, startRef, pathStart, path.pimpl_->requestedEnds[i], @@ -1574,7 +1579,7 @@ T PathFinder::Impl::tryStep(const T& start, const T& end, bool allowSliding) { dtStatus startStatus = 0, endStatus = 0; dtPolyRef startRef = 0, endRef = 0; - vec3f pathStart; + Mn::Vector3 pathStart; std::tie(startStatus, startRef, pathStart) = projectToPoly(start, navQuery_.get(), filter_.get()); std::tie(endStatus, endRef, std::ignore) = @@ -1588,7 +1593,7 @@ T PathFinder::Impl::tryStep(const T& start, const T& end, bool allowSliding) { return start; } - vec3f endPoint; + Mn::Vector3 endPoint; int numPolys = 0; navQuery_->moveAlongSurface(startRef, pathStart.data(), end.data(), filter_.get(), endPoint.data(), polys, &numPolys, @@ -1627,20 +1632,21 @@ T PathFinder::Impl::tryStep(const T& start, const T& end, bool allowSliding) { navMesh_->getTileAndPolyByRefUnsafe(polys[numPolys - 1], &tile, &poly); // Calculate the center of the polygon we want the points to be in - vec3f polyCenter = vec3f::Zero(); + Mn::Vector3 polyCenter; for (int iVert = 0; iVert < poly->vertCount; ++iVert) { - polyCenter += Eigen::Map( + // auto idx = poly->verts[iVert]; + polyCenter += Mn::Vector3::from( &tile->verts[static_cast(poly->verts[iVert]) * 3]); } polyCenter /= poly->vertCount; constexpr float nudgeDistance = 1e-4; // 0.1mm - const vec3f nudgeDir = (polyCenter - endPoint).normalized(); + const Mn::Vector3 nudgeDir = (polyCenter - endPoint).normalized(); // And nudge the point towards the center by a little tiny bit :) endPoint = endPoint + nudgeDistance * nudgeDir; } - return T{std::move(endPoint)}; + return T{endPoint}; } template @@ -1658,7 +1664,7 @@ T PathFinder::Impl::snapPoint(const T& pt, int islandIndex /*=ID_UNDEFINED*/) { } dtStatus status = 0; - vec3f projectedPt; + Mn::Vector3 projectedPt; std::tie(status, std::ignore, projectedPt) = projectToPoly(pt, navQuery_.get(), filter_.get()); @@ -1673,7 +1679,7 @@ T PathFinder::Impl::snapPoint(const T& pt, int islandIndex /*=ID_UNDEFINED*/) { } if (dtStatusSucceed(status)) { - return T{std::move(projectedPt)}; + return T{projectedPt}; } return {Mn::Constants::nan(), Mn::Constants::nan(), Mn::Constants::nan()}; } @@ -1681,7 +1687,7 @@ T PathFinder::Impl::snapPoint(const T& pt, int islandIndex /*=ID_UNDEFINED*/) { template int PathFinder::Impl::getIsland(const T& pt) const { dtStatus status = 0; - vec3f projectedPt; + Mn::Vector3 projectedPt; dtPolyRef polyRef = 0; std::tie(status, polyRef, projectedPt) = projectToPoly(pt, navQuery_.get(), filter_.get()); @@ -1696,7 +1702,7 @@ float PathFinder::Impl::islandRadius(int islandIndex) const { return islandSystem_->islandRadius(islandIndex); } -float PathFinder::Impl::islandRadius(const vec3f& pt) const { +float PathFinder::Impl::islandRadius(const Mn::Vector3& pt) const { dtPolyRef ptRef = 0; dtStatus status = 0; std::tie(status, ptRef, std::ignore) = @@ -1708,36 +1714,36 @@ float PathFinder::Impl::islandRadius(const vec3f& pt) const { } float PathFinder::Impl::distanceToClosestObstacle( - const vec3f& pt, + const Mn::Vector3& pt, const float maxSearchRadius /*= 2.0*/) const { return closestObstacleSurfacePoint(pt, maxSearchRadius).hitDist; } HitRecord PathFinder::Impl::closestObstacleSurfacePoint( - const vec3f& pt, + const Mn::Vector3& pt, const float maxSearchRadius /*= 2.0*/) const { dtPolyRef ptRef = 0; dtStatus status = 0; - vec3f polyPt; + Mn::Vector3 polyPt; std::tie(status, ptRef, polyPt) = projectToPoly(pt, navQuery_.get(), filter_.get()); if (status != DT_SUCCESS || ptRef == 0) { - return {vec3f(0, 0, 0), vec3f(0, 0, 0), + return {Mn::Vector3(0, 0, 0), Mn::Vector3(0, 0, 0), std::numeric_limits::infinity()}; } - vec3f hitPos, hitNormal; + Mn::Vector3 hitPos, hitNormal; float hitDist = Mn::Constants::nan(); navQuery_->findDistanceToWall(ptRef, polyPt.data(), maxSearchRadius, filter_.get(), &hitDist, hitPos.data(), hitNormal.data()); - return {std::move(hitPos), std::move(hitNormal), hitDist}; + return {hitPos, hitNormal, hitDist}; } -bool PathFinder::Impl::isNavigable(const vec3f& pt, +bool PathFinder::Impl::isNavigable(const Mn::Vector3& pt, const float maxYDelta /*= 0.5*/) const { dtPolyRef ptRef = 0; dtStatus status = 0; - vec3f polyPt; + Mn::Vector3 polyPt; std::tie(status, ptRef, polyPt) = projectToPoly(pt, navQuery_.get(), filter_.get()); @@ -1745,22 +1751,19 @@ bool PathFinder::Impl::isNavigable(const vec3f& pt, return false; if (std::abs(polyPt[1] - pt[1]) > maxYDelta || - (Eigen::Vector2f(pt[0], pt[2]) - Eigen::Vector2f(polyPt[0], polyPt[2])) - .norm() > 1e-2) + (Mn::Vector2(pt[0], pt[2]) - Mn::Vector2(polyPt[0], polyPt[2])).length() > + 1e-2f) return false; return true; } -typedef Eigen::Matrix MatrixXb; - -Eigen::Matrix -PathFinder::Impl::getTopDownView(const float metersPerPixel, - const float height, - const float eps) const { - std::pair mapBounds = bounds(); - vec3f bound1 = std::move(mapBounds.first); - vec3f bound2 = std::move(mapBounds.second); +MatrixXb PathFinder::Impl::getTopDownView(const float metersPerPixel, + const float height, + const float eps) const { + std::pair mapBounds = bounds(); + Mn::Vector3 bound1 = mapBounds.first; + Mn::Vector3 bound2 = mapBounds.second; float xspan = std::abs(bound1[0] - bound2[0]); float zspan = std::abs(bound1[2] - bound2[2]); @@ -1774,7 +1777,7 @@ PathFinder::Impl::getTopDownView(const float metersPerPixel, float curx = startx; for (int h = 0; h < zResolution; ++h) { for (int w = 0; w < xResolution; ++w) { - vec3f point = vec3f(curx, height, curz); + Mn::Vector3 point = Mn::Vector3(curx, height, curz); topdownMap(h, w) = isNavigable(point, eps); curx = curx + metersPerPixel; } @@ -1785,14 +1788,12 @@ PathFinder::Impl::getTopDownView(const float metersPerPixel, return topdownMap; } -typedef Eigen::Matrix MatrixXi; - MatrixXi PathFinder::Impl::getTopDownIslandView(const float metersPerPixel, const float height, const float eps) const { - std::pair mapBounds = bounds(); - vec3f bound1 = std::move(mapBounds.first); - vec3f bound2 = std::move(mapBounds.second); + std::pair mapBounds = bounds(); + Mn::Vector3 bound1 = mapBounds.first; + Mn::Vector3 bound2 = mapBounds.second; float xspan = std::abs(bound1[0] - bound2[0]); float zspan = std::abs(bound1[2] - bound2[2]); @@ -1806,7 +1807,7 @@ MatrixXi PathFinder::Impl::getTopDownIslandView(const float metersPerPixel, float curx = startx; for (int h = 0; h < zResolution; ++h) { for (int w = 0; w < xResolution; ++w) { - vec3f point = vec3f(curx, height, curz); + Mn::Vector3 point = Mn::Vector3(curx, height, curz); if (isNavigable(point, eps)) { // get the island topdownMap(h, w) = getIsland(point); @@ -1829,7 +1830,7 @@ assets::MeshData::ptr PathFinder::Impl::getNavMeshData( if (islandMeshData_.find(islandIndex) == islandMeshData_.end() && isLoaded()) { assets::MeshData::ptr curIslandMeshData = assets::MeshData::create(); - std::vector& vbo = curIslandMeshData->vbo; + std::vector& vbo = curIslandMeshData->vbo; std::vector& ibo = curIslandMeshData->ibo; // Iterate over all tiles @@ -1873,7 +1874,7 @@ assets::MeshData::ptr PathFinder::Impl::getNavMeshData( return islandMeshData_[islandIndex]; } -PathFinder::PathFinder() : pimpl_{spimpl::make_unique_impl()} {}; +PathFinder::PathFinder() : pimpl_{spimpl::make_unique_impl()} {} bool PathFinder::build(const NavMeshSettings& bs, const float* verts, @@ -1889,13 +1890,14 @@ bool PathFinder::build(const NavMeshSettings& bs, return pimpl_->build(bs, mesh); } -vec3f PathFinder::getRandomNavigablePoint(const int maxTries /*= 10*/, - int islandIndex /*= ID_UNDEFINED*/) { +Mn::Vector3 PathFinder::getRandomNavigablePoint( + const int maxTries /*= 10*/, + int islandIndex /*= ID_UNDEFINED*/) { return pimpl_->getRandomNavigablePoint(maxTries, islandIndex); } -vec3f PathFinder::getRandomNavigablePointAroundSphere( - const vec3f& circleCenter, +Mn::Vector3 PathFinder::getRandomNavigablePointAroundSphere( + const Mn::Vector3& circleCenter, const float radius, const int maxTries, int islandIndex /*= ID_UNDEFINED*/) { @@ -1911,7 +1913,6 @@ bool PathFinder::findPath(MultiGoalShortestPath& path) { return pimpl_->findPath(path); } -template vec3f PathFinder::tryStep(const vec3f&, const vec3f&); template Mn::Vector3 PathFinder::tryStep(const Mn::Vector3&, const Mn::Vector3&); @@ -1920,7 +1921,6 @@ T PathFinder::tryStep(const T& start, const T& end) { return pimpl_->tryStep(start, end, /*allowSliding=*/true); } -template vec3f PathFinder::tryStepNoSliding(const vec3f&, const vec3f&); template Mn::Vector3 PathFinder::tryStepNoSliding( const Mn::Vector3&, const Mn::Vector3&); @@ -1930,11 +1930,9 @@ T PathFinder::tryStepNoSliding(const T& start, const T& end) { return pimpl_->tryStep(start, end, /*allowSliding=*/false); } -template vec3f PathFinder::snapPoint(const vec3f& pt, int islandIndex); template Mn::Vector3 PathFinder::snapPoint(const Mn::Vector3& pt, int islandIndex); -template int PathFinder::getIsland(const vec3f& pt); template int PathFinder::getIsland(const Mn::Vector3& pt); template @@ -1963,7 +1961,7 @@ void PathFinder::seed(uint32_t newSeed) { return pimpl_->seed(newSeed); } -float PathFinder::islandRadius(const vec3f& pt) const { +float PathFinder::islandRadius(const Mn::Vector3& pt) const { return pimpl_->islandRadius(pt); } @@ -1975,18 +1973,19 @@ int PathFinder::numIslands() const { return pimpl_->numIslands(); } -float PathFinder::distanceToClosestObstacle(const vec3f& pt, +float PathFinder::distanceToClosestObstacle(const Mn::Vector3& pt, const float maxSearchRadius) const { return pimpl_->distanceToClosestObstacle(pt, maxSearchRadius); } HitRecord PathFinder::closestObstacleSurfacePoint( - const vec3f& pt, + const Mn::Vector3& pt, const float maxSearchRadius) const { return pimpl_->closestObstacleSurfacePoint(pt, maxSearchRadius); } -bool PathFinder::isNavigable(const vec3f& pt, const float maxYDelta) const { +bool PathFinder::isNavigable(const Mn::Vector3& pt, + const float maxYDelta) const { return pimpl_->isNavigable(pt, maxYDelta); } @@ -1994,21 +1993,19 @@ float PathFinder::getNavigableArea(int islandIndex /*= ID_UNDEFINED*/) const { return pimpl_->getNavigableArea(islandIndex); } -std::pair PathFinder::bounds() const { +std::pair PathFinder::bounds() const { return pimpl_->bounds(); } -Eigen::Matrix PathFinder::getTopDownView( - const float metersPerPixel, - const float height, - const float eps) { +MatrixXb PathFinder::getTopDownView(const float metersPerPixel, + const float height, + const float eps) { return pimpl_->getTopDownView(metersPerPixel, height, eps); } -Eigen::Matrix -PathFinder::getTopDownIslandView(const float metersPerPixel, - const float height, - const float eps) { +MatrixXi PathFinder::getTopDownIslandView(const float metersPerPixel, + const float height, + const float eps) { return pimpl_->getTopDownIslandView(metersPerPixel, height, eps); } diff --git a/src/esp/nav/PathFinder.h b/src/esp/nav/PathFinder.h index bf782cb232..64fffc81bd 100644 --- a/src/esp/nav/PathFinder.h +++ b/src/esp/nav/PathFinder.h @@ -6,6 +6,7 @@ #define ESP_NAV_PATHFINDER_H_ #include +#include #include #include @@ -21,6 +22,10 @@ struct MeshData; //! NavMesh namespace namespace nav { +typedef Eigen::Matrix MatrixXi; + +typedef Eigen::Matrix MatrixXb; + class PathFinder; /** @@ -28,9 +33,9 @@ class PathFinder; */ struct HitRecord { //! World position of the closest obstacle. - vec3f hitPos; + Magnum::Vector3 hitPos; //! Normal of the navmesh at the obstacle in xz plane. - vec3f hitNormal; + Magnum::Vector3 hitNormal; //! Distance from query point to closest obstacle. Inf if no valid point was //! found. float hitDist{}; @@ -44,12 +49,12 @@ struct ShortestPath { /** * @brief The starting point for the path */ - vec3f requestedStart; + Magnum::Vector3 requestedStart; /** * @brief The ending point for the path */ - vec3f requestedEnd; + Magnum::Vector3 requestedEnd; /** * @brief A list of points that specify the shortest path on the navigation @@ -57,7 +62,7 @@ struct ShortestPath { * * @note Will be empty if no path exists */ - std::vector points; + std::vector points; /** * @brief The geodesic distance between @ref requestedStart and @ref @@ -80,14 +85,14 @@ struct MultiGoalShortestPath { /** * @brief The starting point for the path */ - vec3f requestedStart; + Magnum::Vector3 requestedStart; /** * @brief Set the list of desired potential end points */ - void setRequestedEnds(const std::vector& newEnds); + void setRequestedEnds(const std::vector& newEnds); - const std::vector& getRequestedEnds() const; + const std::vector& getRequestedEnds() const; /** * @brief A list of points that specify the shortest path on the navigation @@ -96,7 +101,7 @@ struct MultiGoalShortestPath { * * Will be empty if no path exists */ - std::vector points; + std::vector points; /** * @brief The geodesic distance @@ -386,8 +391,8 @@ class PathFinder { * the returned point will be `{NAN, NAN, NAN}`. Use @ref * isNavigable to check if the point is navigable. */ - vec3f getRandomNavigablePoint(int maxTries = 10, - int islandIndex = ID_UNDEFINED); + Magnum::Vector3 getRandomNavigablePoint(int maxTries = 10, + int islandIndex = ID_UNDEFINED); /** * @brief Returns a random navigable point within a specified radius about a @@ -406,10 +411,11 @@ class PathFinder { * the returned point will be `{NAN, NAN, NAN}`. Use @ref * isNavigable to check if the point is navigable. */ - vec3f getRandomNavigablePointAroundSphere(const vec3f& circleCenter, - float radius, - int maxTries = 10, - int islandIndex = ID_UNDEFINED); + Magnum::Vector3 getRandomNavigablePointAroundSphere( + const Magnum::Vector3& circleCenter, + float radius, + int maxTries = 10, + int islandIndex = ID_UNDEFINED); /** * @brief Finds the shortest path between two points on the navigation mesh @@ -543,7 +549,7 @@ class PathFinder { * * @return Heuristic size of the connected component. */ - float islandRadius(const vec3f& pt) const; + float islandRadius(const Magnum::Vector3& pt) const; /** * @brief returns the size of the specified connected component. @@ -570,14 +576,14 @@ class PathFinder { * @return The distance to the closest non-navigable location or @ref * maxSearchRadius if all locations within @ref maxSearchRadius are navigable */ - float distanceToClosestObstacle(const vec3f& pt, + float distanceToClosestObstacle(const Magnum::Vector3& pt, float maxSearchRadius = 2.0) const; /** * @brief Same as @ref distanceToClosestObstacle but returns additional * information. */ - HitRecord closestObstacleSurfacePoint(const vec3f& pt, + HitRecord closestObstacleSurfacePoint(const Magnum::Vector3& pt, float maxSearchRadius = 2.0) const; /** @@ -593,7 +599,7 @@ class PathFinder { * * @return Whether or not @ref pt is navigable */ - bool isNavigable(const vec3f& pt, float maxYDelta = 0.5) const; + bool isNavigable(const Magnum::Vector3& pt, float maxYDelta = 0.5) const; /** * Compute and return the total area of all NavMesh polygons. @@ -608,7 +614,7 @@ class PathFinder { /** * @return The axis aligned bounding box containing the navigation mesh. */ - std::pair bounds() const; + std::pair bounds() const; /** * @brief Get a 2D grid marking navigable and non-navigable cells at a @@ -627,8 +633,7 @@ class PathFinder { * * @return The 2D grid marking cells as navigable or not. */ - Eigen::Matrix - getTopDownView(float metersPerPixel, float height, float eps = 0.5); + MatrixXb getTopDownView(float metersPerPixel, float height, float eps = 0.5); /** * @brief Get a 2D grid marking island index for navigable cells and -1 for @@ -644,8 +649,9 @@ class PathFinder { * * @return The 2D grid marking cell islands or -1 for not navigable. */ - Eigen::Matrix - getTopDownIslandView(float metersPerPixel, float height, float eps = 0.5); + MatrixXi getTopDownIslandView(float metersPerPixel, + float height, + float eps = 0.5); /** * @brief Returns a MeshData object containing triangulated NavMesh polys. diff --git a/src/esp/scene/GibsonSemanticScene.cpp b/src/esp/scene/GibsonSemanticScene.cpp index 31cc7a2cc8..e36ea87e78 100644 --- a/src/esp/scene/GibsonSemanticScene.cpp +++ b/src/esp/scene/GibsonSemanticScene.cpp @@ -17,8 +17,9 @@ namespace scene { constexpr int kMaxIds = 10000; /* We shouldn't ever need more than this. */ -bool SemanticScene:: - loadGibsonHouse(const std::string& houseFilename, SemanticScene& scene, const quatf& rotation /* = quatf::FromTwoVectors(-vec3f::UnitZ(), geo::ESP_GRAVITY) */) { +bool SemanticScene::loadGibsonHouse(const std::string& houseFilename, + SemanticScene& scene, + const Mn::Quaternion& rotation) { if (!checkFileExists(houseFilename, "loadGibsonHouse")) { return false; } @@ -33,7 +34,7 @@ bool SemanticScene:: bool SemanticScene::buildGibsonHouse(const io::JsonDocument& jsonDoc, SemanticScene& scene, - const quatf& rotation) { + const Mn::Quaternion& rotation) { scene.categories_.clear(); scene.objects_.clear(); @@ -74,19 +75,24 @@ bool SemanticScene::buildGibsonHouse(const io::JsonDocument& jsonDoc, if (jsonLocIter != jsonObject.MemberEnd()) { // if (jsonObject.HasMember("location")) { const auto& jsonCenter = jsonLocIter->value; - vec3f center = rotation * io::jsonToVec3f(jsonCenter); - vec3f size = vec3f::Zero(); + Mn::Vector3 jsonVecRes; + io::fromJsonValue(jsonCenter, jsonVecRes); + Mn::Vector3 center = rotation.transformVectorNormalized(jsonVecRes); + Mn::Vector3 size; io::JsonGenericValue::ConstMemberIterator jsonSizeIter = jsonObject.FindMember("size"); if (jsonSizeIter != jsonObject.MemberEnd()) { const auto& jsonSize = jsonSizeIter->value; // Rotating sizes - size = (rotation * io::jsonToVec3f(jsonSize)).array().abs(); + Mn::Vector3 sizeVec; + io::fromJsonValue(jsonSize, sizeVec); + + size = abs((rotation.transformVectorNormalized(sizeVec))); } else { ESP_WARNING() << "Object size from" << categoryName << "isn't provided."; } - object->setObb(center, size, quatf::Identity()); + object->setObb(center, size, Mn::Quaternion(Mn::Math::IdentityInit)); } else { ESP_WARNING() << "Object center coordinates from" << categoryName << "aren't provided."; diff --git a/src/esp/scene/HM3DSemanticScene.cpp b/src/esp/scene/HM3DSemanticScene.cpp index bfb0b6b6de..aa092df2ab 100644 --- a/src/esp/scene/HM3DSemanticScene.cpp +++ b/src/esp/scene/HM3DSemanticScene.cpp @@ -15,11 +15,9 @@ namespace Cr = Corrade; namespace esp { namespace scene { -bool SemanticScene::loadHM3DHouse( - const std::string& houseFilename, - SemanticScene& scene, - const quatf& rotation /* = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY) */ ) { +bool SemanticScene::loadHM3DHouse(const std::string& houseFilename, + SemanticScene& scene, + const Mn::Quaternion& rotation) { if (!checkFileExists(houseFilename, "loadHM3DHouse")) { return false; } @@ -92,7 +90,7 @@ void buildInstanceRegionCategory( bool SemanticScene::buildHM3DHouse(std::ifstream& ifs, SemanticScene& scene, - const quatf& /*rotation*/) { + const Mn::Quaternion& /*rotation*/) { // temp constructs std::map objInstance; std::map regions; diff --git a/src/esp/scene/Mp3dSemanticScene.cpp b/src/esp/scene/Mp3dSemanticScene.cpp index 4b86d33131..26a1454a95 100644 --- a/src/esp/scene/Mp3dSemanticScene.cpp +++ b/src/esp/scene/Mp3dSemanticScene.cpp @@ -80,11 +80,9 @@ std::string Mp3dRegionCategory::name(const std::string&) const { return kRegionCategoryMap.at(labelCode_); } -bool SemanticScene::loadMp3dHouse( - const std::string& houseFilename, - SemanticScene& scene, - const quatf& rotation /* = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY) */ ) { +bool SemanticScene::loadMp3dHouse(const std::string& houseFilename, + SemanticScene& scene, + const Mn::Quaternion& rotation) { if (!checkFileExists(houseFilename, "loadMp3dHouse")) { return false; } @@ -104,51 +102,58 @@ bool SemanticScene::loadMp3dHouse( bool SemanticScene::buildMp3dHouse(std::ifstream& ifs, SemanticScene& scene, - const quatf& rotation) { - const bool hasWorldRotation = !rotation.isApprox(quatf::Identity()); + const Mn::Quaternion& rotation) { + const bool hasWorldRotation = + rotation != Mn::Quaternion(Mn::Math::IdentityInit); - auto getVec3f = [&](const std::vector& tokens, int offset, - bool applyRotation = true) -> vec3f { + auto getVector3 = [&](const std::vector& tokens, int offset, + bool applyRotation = true) -> Mn::Vector3 { const float x = std::stof(tokens[offset]); const float y = std::stof(tokens[offset + 1]); const float z = std::stof(tokens[offset + 2]); - vec3f p = vec3f(x, y, z); + Mn::Vector3 p(x, y, z); if (applyRotation && hasWorldRotation) { - p = rotation * p; + p = rotation.transformVectorNormalized(p); } return p; }; auto getBBox = [&](const std::vector& tokens, - int offset) -> box3f { + int offset) -> Mn::Range3D { // Get the bounding box without rotating as rotating min/max is odd - box3f sceneBox{getVec3f(tokens, offset, /*applyRotation=*/false), - getVec3f(tokens, offset + 3, /*applyRotation=*/false)}; + Mn::Range3D sceneBox{ + getVector3(tokens, offset, /*applyRotation=*/false), + getVector3(tokens, offset + 3, /*applyRotation=*/false)}; if (!hasWorldRotation) return sceneBox; // Apply the rotation to center/sizes - const vec3f worldCenter = rotation * sceneBox.center(); - const vec3f worldHalfSizes = - (rotation * sceneBox.sizes()).array().abs().matrix() / 2.0f; + const Mn::Vector3 worldCenter = + rotation.transformVectorNormalized(sceneBox.center()); + + const Mn::Vector3 worldHalfSizes = + abs(rotation.transformVectorNormalized(sceneBox.size())) / 2.0f; // Then remake the box with min/max computed from rotated center/size - return box3f{(worldCenter - worldHalfSizes).eval(), - (worldCenter + worldHalfSizes).eval()}; + return Mn::Range3D{(worldCenter - worldHalfSizes), + (worldCenter + worldHalfSizes)}; }; auto getOBB = [&](const std::vector& tokens, int offset) { - const vec3f center = getVec3f(tokens, offset); + const Mn::Vector3 center = getVector3(tokens, offset); - // Don't need to apply rotation here, it'll already be added in by getVec3f - mat3f boxRotation; - boxRotation.col(0) << getVec3f(tokens, offset + 3); - boxRotation.col(1) << getVec3f(tokens, offset + 6); - boxRotation.col(2) << boxRotation.col(0).cross(boxRotation.col(1)); + // Don't need to apply rotation here, it'll already be added in by + // getVector3 + Mn::Matrix3 boxRotation; + boxRotation[0] = getVector3(tokens, offset + 3); + boxRotation[1] = getVector3(tokens, offset + 6); + boxRotation[2] = Mn::Math::cross(boxRotation[0], boxRotation[1]); // Don't apply the world rotation here, that'll get added by boxRotation - const vec3f radius = getVec3f(tokens, offset + 9, /*applyRotation=*/false); + const Mn::Vector3 radius = + getVector3(tokens, offset + 9, /*applyRotation=*/false); - return geo::OBB(center, 2 * radius, quatf(boxRotation)); + return geo::OBB(center, 2 * radius, + Mn::Quaternion::fromMatrix(boxRotation)); }; scene.categories_.clear(); @@ -191,7 +196,7 @@ bool SemanticScene::buildMp3dHouse(std::ifstream& ifs, level->index_ = std::stoi(tokens[1]); // NOTE tokens[2] is number of regions in level which we don't need level->labelCode_ = tokens[3]; - level->position_ = getVec3f(tokens, 4); + level->position_ = getVector3(tokens, 4); level->bbox_ = getBBox(tokens, 7); break; } @@ -203,7 +208,7 @@ bool SemanticScene::buildMp3dHouse(std::ifstream& ifs, region->index_ = std::stoi(tokens[1]); region->parentIndex_ = std::stoi(tokens[2]); region->category_ = std::make_shared(tokens[5][0]); - region->position_ = getVec3f(tokens, 6); + region->position_ = getVector3(tokens, 6); region->bbox_ = getBBox(tokens, 9); if (region->parentIndex_ >= 0) { region->level_ = scene.levels_[region->parentIndex_]; diff --git a/src/esp/scene/ReplicaSemanticScene.cpp b/src/esp/scene/ReplicaSemanticScene.cpp index 06c01cd40e..d2358371a7 100644 --- a/src/esp/scene/ReplicaSemanticScene.cpp +++ b/src/esp/scene/ReplicaSemanticScene.cpp @@ -5,23 +5,21 @@ #include "ReplicaSemanticScene.h" #include "SemanticScene.h" +#include + #include #include #include "esp/io/Json.h" -#include -#include - namespace esp { namespace scene { constexpr int kMaxIds = 10000; /* We shouldn't every need more than this. */ -bool SemanticScene::loadReplicaHouse( - const std::string& houseFilename, - SemanticScene& scene, - const quatf& worldRotation /* = quatf::Identity() */) { +bool SemanticScene::loadReplicaHouse(const std::string& houseFilename, + SemanticScene& scene, + const Magnum::Quaternion& worldRotation) { if (!checkFileExists(houseFilename, "loadReplicaHouse")) { return false; } @@ -44,7 +42,7 @@ bool SemanticScene::loadReplicaHouse( bool SemanticScene::buildReplicaHouse(const io::JsonDocument& jsonDoc, SemanticScene& scene, bool objectsExist, - const quatf& worldRotation) { + const Magnum::Quaternion& worldRotation) { scene.categories_.clear(); scene.objects_.clear(); @@ -80,7 +78,8 @@ bool SemanticScene::buildReplicaHouse(const io::JsonDocument& jsonDoc, scene.elementCounts_["objects"] = objects.Size(); // construct rotation matrix to be used to construct transform - const Eigen::Isometry3f worldRotationMat{worldRotation.normalized()}; + const auto worldRotationMat = + Mn::Matrix4::from(worldRotation.normalized().toMatrix(), {}); for (const auto& jsonObject : objects) { SemanticObject::ptr object = SemanticObject::create(); int id = jsonObject["id"].GetInt(); @@ -102,25 +101,27 @@ bool SemanticScene::buildReplicaHouse(const io::JsonDocument& jsonDoc, } const auto& obb = jsonObject["oriented_bbox"]; - const vec3f aabbCenter = io::jsonToVec3f(obb["abb"]["center"]); - const vec3f aabbSizes = io::jsonToVec3f(obb["abb"]["sizes"]); - - const vec3f translationBoxToWorld = - io::jsonToVec3f(obb["orientation"]["translation"]); - - std::vector rotationBoxToWorldCoeffs; - io::toFloatVector(obb["orientation"]["rotation"], - &rotationBoxToWorldCoeffs); - const Eigen::Map rotationBoxToWorld(rotationBoxToWorldCoeffs.data()); - - Eigen::Isometry3f transformBox{rotationBoxToWorld.normalized()}; - transformBox *= Eigen::Translation3f(translationBoxToWorld); - - const Eigen::Isometry3f transformBoxToWorld{worldRotationMat * - transformBox}; - - object->obb_ = geo::OBB{transformBoxToWorld * aabbCenter, aabbSizes, - quatf{transformBoxToWorld.linear()}.normalized()}; + Mn::Vector3 aabbCenter; + io::fromJsonValue(obb["abb"]["center"], aabbCenter); + Mn::Vector3 aabbSizes; + io::fromJsonValue(obb["abb"]["sizes"], aabbSizes); + Mn::Vector3 translationBoxToWorld; + io::fromJsonValue(obb["orientation"]["translation"], translationBoxToWorld); + // 4th element is scalar in json + Mn::Vector4 rotBoxToWorldCoeffs; + io::fromJsonValue(obb["orientation"]["rotation"], rotBoxToWorldCoeffs); + + const Mn::Quaternion rotationBoxToWorld(rotBoxToWorldCoeffs.xyz(), + rotBoxToWorldCoeffs.w()); + + const Mn::Matrix4 transformBox = Mn::Matrix4::from( + rotationBoxToWorld.normalized().toMatrix(), translationBoxToWorld); + + const auto transformBoxToWorld{worldRotationMat * transformBox}; + + object->obb_ = geo::OBB{ + transformBoxToWorld.transformVector(aabbCenter), aabbSizes, + Mn::Quaternion::fromMatrix(transformBoxToWorld.rotationNormalized())}; scene.objects_[id] = std::move(object); } diff --git a/src/esp/scene/SemanticScene.cpp b/src/esp/scene/SemanticScene.cpp index f7403cd3c6..1ea05e1906 100644 --- a/src/esp/scene/SemanticScene.cpp +++ b/src/esp/scene/SemanticScene.cpp @@ -3,7 +3,6 @@ // LICENSE file in the root directory of this source tree. #include "SemanticScene.h" -#include #include "GibsonSemanticScene.h" #include "Mp3dSemanticScene.h" #include "ReplicaSemanticScene.h" @@ -23,8 +22,11 @@ namespace esp { namespace scene { -bool SemanticScene:: - loadSemanticSceneDescriptor(const std::shared_ptr& semanticAttr, SemanticScene& scene, const quatf& rotation /* = quatf::FromTwoVectors(-vec3f::UnitZ(), geo::ESP_GRAVITY) */) { +bool SemanticScene::loadSemanticSceneDescriptor( + const std::shared_ptr& + semanticAttr, + SemanticScene& scene, + const Mn::Quaternion& rotation) { const std::string ssdFileName = semanticAttr != nullptr ? semanticAttr->getSemanticDescriptorFullPath() : ""; @@ -231,7 +233,7 @@ bool SemanticRegion::contains(const Mn::Vector3& pt) const { return false; } // Next check bbox - if (!bbox_.contains(Mn::EigenIntegration::cast(pt))) { + if (!bbox_.contains(pt)) { return false; } // Lastly, count casts across edges. @@ -257,8 +259,7 @@ bool SemanticRegion::contains(const Mn::Vector3& pt) const { } // SemanticRegion::contains void SemanticRegion::setBBox(const Mn::Vector3& min, const Mn::Vector3& max) { - bbox_ = box3f(Mn::EigenIntegration::cast(min), - Mn::EigenIntegration::cast(max)); + bbox_ = {min, max}; } // SemanticRegion::setBBox namespace { @@ -292,8 +293,7 @@ CCSemanticObject::ptr buildCCSemanticObjForSetOfVerts( auto obj = std::make_shared(CCSemanticObject(colorInt, setOfIDXs)); // set obj's bounding box - obj->setObb(Mn::EigenIntegration::cast(center), - Mn::EigenIntegration::cast(dims), quatf::Identity()); + obj->setObb(center, dims, Mn::Quaternion(Mn::Math::IdentityInit)); return obj; } // buildCCSemanticObjForSetOfVerts @@ -590,8 +590,7 @@ std::vector SemanticScene::buildSemanticOBBs( ssdObj.id(), vertCounts[semanticID], center.x(), center.y(), center.z(), dims.x(), dims.y(), dims.z()); } - ssdObj.setObb(Mn::EigenIntegration::cast(center), - Mn::EigenIntegration::cast(dims)); + ssdObj.setObb(center, dims); } // return listing of semantic object idxs that have no presence in the mesh return unMappedObjectIDXs; diff --git a/src/esp/scene/SemanticScene.h b/src/esp/scene/SemanticScene.h index 8cbc752f98..6ff0de5630 100644 --- a/src/esp/scene/SemanticScene.h +++ b/src/esp/scene/SemanticScene.h @@ -14,6 +14,7 @@ #include #include "esp/core/Esp.h" +#include "esp/core/Utility.h" #include "esp/geo/OBB.h" #include "esp/io/Json.h" @@ -25,6 +26,8 @@ class SemanticAttributes; } // namespace metadata namespace scene { +namespace Mn = Magnum; + //! Represents a semantic category class SemanticCategory { public: @@ -52,7 +55,7 @@ class SemanticScene { public: ~SemanticScene() { ESP_DEBUG() << "Deconstructing SemanticScene"; } //! return axis aligned bounding box of this House - box3f aabb() const { return bbox_; } + Mn::Range3D aabb() const { return bbox_; } //! return total number of given element type int count(const std::string& element) const { @@ -108,8 +111,8 @@ class SemanticScene { const std::shared_ptr& semanticAttr, SemanticScene& scene, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, geo::ESP_GRAVITY)); /** * @brief Attempt to load SemanticScene from a Gibson dataset house format @@ -120,11 +123,11 @@ class SemanticScene { * @param rotation rotation to apply to semantic scene upon load. * @return successfully loaded */ - static bool loadGibsonHouse( - const std::string& filename, - SemanticScene& scene, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + static bool loadGibsonHouse(const std::string& filename, + SemanticScene& scene, + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Attempt to load SemanticScene from a HM3D dataset house @@ -138,9 +141,9 @@ class SemanticScene { */ static bool loadHM3DHouse(const std::string& filename, SemanticScene& scene, - CORRADE_UNUSED const quatf& rotation = - quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + CORRADE_UNUSED const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Attempt to load SemanticScene from a Matterport3D dataset house @@ -151,11 +154,11 @@ class SemanticScene { * @param rotation rotation to apply to semantic scene upon load. * @return successfully loaded */ - static bool loadMp3dHouse( - const std::string& filename, - SemanticScene& scene, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + static bool loadMp3dHouse(const std::string& filename, + SemanticScene& scene, + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Attempt to load SemanticScene from a Replica dataset house format @@ -166,11 +169,11 @@ class SemanticScene { * @param rotation rotation to apply to semantic scene upon load. * @return successfully loaded */ - static bool loadReplicaHouse( - const std::string& filename, - SemanticScene& scene, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + static bool loadReplicaHouse(const std::string& filename, + SemanticScene& scene, + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Builds a mapping of connected component-driven bounding boxes (via @@ -345,9 +348,9 @@ class SemanticScene { */ static bool buildHM3DHouse(std::ifstream& ifs, SemanticScene& scene, - CORRADE_UNUSED const quatf& rotation = - quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + CORRADE_UNUSED const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Build the mp3 semantic data from the passed file stream. File being * streamed is expected to be appropriate format. @@ -357,11 +360,11 @@ class SemanticScene { * @return successfully built. Currently only returns true, but retaining * return value for future support. */ - static bool buildMp3dHouse( - std::ifstream& ifs, - SemanticScene& scene, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + static bool buildMp3dHouse(std::ifstream& ifs, + SemanticScene& scene, + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Build SemanticScene from a Gibson dataset house JSON. JSON is @@ -372,11 +375,11 @@ class SemanticScene { * @return successfully built. Currently only returns true, but retaining * return value for future support. */ - static bool buildGibsonHouse( - const io::JsonDocument& jsonDoc, - SemanticScene& scene, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + static bool buildGibsonHouse(const io::JsonDocument& jsonDoc, + SemanticScene& scene, + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); /** * @brief Build SemanticScene from a Replica dataset house JSON. JSON is @@ -389,12 +392,12 @@ class SemanticScene { * @return successfully built. Currently only returns true, but retaining * return value for future support. */ - static bool buildReplicaHouse( - const io::JsonDocument& jsonDoc, - SemanticScene& scene, - bool objectsExist, - const quatf& rotation = quatf::FromTwoVectors(-vec3f::UnitZ(), - geo::ESP_GRAVITY)); + static bool buildReplicaHouse(const io::JsonDocument& jsonDoc, + SemanticScene& scene, + bool objectsExist, + const Magnum::Quaternion& rotation = + Mn::Quaternion::rotation(geo::ESP_FRONT, + geo::ESP_GRAVITY)); // Currently only supported by HM3D semantic files. bool hasVertColors_ = false; @@ -413,7 +416,7 @@ class SemanticScene { std::string name_; std::string label_; - box3f bbox_; + Mn::Range3D bbox_; std::map elementCounts_; std::vector> categories_; std::vector> levels_; @@ -441,7 +444,6 @@ class SemanticLevel { public: virtual ~SemanticLevel() = default; virtual std::string id() const { return std::to_string(index_); } - const std::vector>& regions() const { return regions_; } @@ -450,13 +452,13 @@ class SemanticLevel { return objects_; } - box3f aabb() const { return bbox_; } + Mn::Range3D aabb() const { return bbox_; } protected: int index_{}; std::string labelCode_; - vec3f position_; - box3f bbox_; + Mn::Vector3 position_; + Mn::Range3D bbox_; std::vector> objects_; std::vector> regions_; friend SemanticScene; @@ -515,7 +517,7 @@ class SemanticRegion { void setBBox(const Mn::Vector3& min, const Mn::Vector3& max); - box3f aabb() const { return bbox_; } + Mn::Range3D aabb() const { return bbox_; } const std::vector& getPolyLoopPoints() const { return polyLoopPoints_; @@ -549,8 +551,8 @@ class SemanticRegion { int index_{}; int parentIndex_{}; std::shared_ptr category_; - vec3f position_; - box3f bbox_; + Mn::Vector3 position_; + Mn::Range3D bbox_; std::string name_; @@ -593,15 +595,16 @@ class SemanticObject { SemanticRegion::ptr region() const { return region_; } - box3f aabb() const { return obb_.toAABB(); } + Mn::Range3D aabb() const { return obb_.toAABB(); } geo::OBB obb() const { return obb_; } SemanticCategory::ptr category() const { return category_; } - void setObb(const esp::vec3f& center, - const esp::vec3f& dimensions, - const esp::quatf& rotation = quatf::Identity()) { + void setObb( + const Mn::Vector3& center, + const Mn::Vector3& dimensions, + const Mn::Quaternion& rotation = Mn::Quaternion(Mn::Math::IdentityInit)) { obb_ = geo::OBB{center, dimensions, rotation}; } void setObb(const geo::OBB& otr) { obb_ = geo::OBB{otr}; } diff --git a/src/esp/sensor/RedwoodNoiseModel.cpp b/src/esp/sensor/RedwoodNoiseModel.cpp index 941ba9c235..dabaede110 100644 --- a/src/esp/sensor/RedwoodNoiseModel.cpp +++ b/src/esp/sensor/RedwoodNoiseModel.cpp @@ -73,12 +73,13 @@ RedwoodNoiseModelGPUImpl::~RedwoodNoiseModelGPUImpl() { Eigen::RowMatrixXf RedwoodNoiseModelGPUImpl::simulateFromCPU( const Eigen::Ref depth) { CudaDeviceContext ctx{gpuDeviceId_}; + const auto numRows = depth.rows(); + const auto numCols = depth.cols(); + Eigen::RowMatrixXf noisyDepth(numRows, numCols); - Eigen::RowMatrixXf noisyDepth(depth.rows(), depth.cols()); - - impl::simulateFromCPU(maxThreadsPerBlock_, warpSize_, depth.data(), - depth.rows(), depth.cols(), devModel_, curandStates_, - noiseMultiplier_, noisyDepth.data()); + impl::simulateFromCPU(maxThreadsPerBlock_, warpSize_, depth.data(), numRows, + numCols, devModel_, curandStates_, noiseMultiplier_, + noisyDepth.data()); return noisyDepth; } diff --git a/src/esp/sim/BatchPlayerImplementation.cpp b/src/esp/sim/BatchPlayerImplementation.cpp index 699466925f..3997d1deab 100644 --- a/src/esp/sim/BatchPlayerImplementation.cpp +++ b/src/esp/sim/BatchPlayerImplementation.cpp @@ -6,7 +6,10 @@ #include +#include #include +#include +#include namespace { bool isSupportedRenderAsset(const Corrade::Containers::StringView& filepath) { diff --git a/src/esp/sim/BatchReplayRenderer.cpp b/src/esp/sim/BatchReplayRenderer.cpp index f03c691556..12dc9e5db6 100644 --- a/src/esp/sim/BatchReplayRenderer.cpp +++ b/src/esp/sim/BatchReplayRenderer.cpp @@ -9,6 +9,7 @@ #include "esp/sensor/CameraSensor.h" #include +#include #include #include #include diff --git a/src/esp/sim/Simulator.cpp b/src/esp/sim/Simulator.cpp index 549043eb15..31c935e6a2 100644 --- a/src/esp/sim/Simulator.cpp +++ b/src/esp/sim/Simulator.cpp @@ -11,9 +11,9 @@ #include #include #include -#include #include #include +#include #include "esp/core/Esp.h" #include "esp/gfx/CubeMapCamera.h" @@ -868,22 +868,20 @@ assets::MeshData::ptr Simulator::getJoinedMesh( // collect mesh components from all objects and then merge them. // Each mesh component could be duplicated multiple times w/ different // transforms. - std::map>> - meshComponentStates; + std::map> meshComponentStates; auto rigidObjMgr = getRigidObjectManager(); // collect RigidObject mesh components for (auto objectID : physicsManager_->getExistingObjectIDs()) { auto objWrapper = rigidObjMgr->getObjectCopyByID(objectID); if (objWrapper->getMotionType() == physics::MotionType::STATIC) { - auto objectTransform = Magnum::EigenIntegration::cast< - Eigen::Transform>( + auto objectTransform = physicsManager_->getObjectVisualSceneNode(objectID) - .absoluteTransformationMatrix()); + .absoluteTransformationMatrix(); const metadata::attributes::ObjectAttributes::cptr initializationTemplate = objWrapper->getInitializationAttributes(); - objectTransform.scale(Magnum::EigenIntegration::cast( - initializationTemplate->getScale())); + objectTransform = + objectTransform * + Mn::Matrix4::scaling(initializationTemplate->getScale()); std::string meshHandle = initializationTemplate->getCollisionAssetFullPath(); if (meshHandle.empty()) { @@ -907,9 +905,8 @@ assets::MeshData::ptr Simulator::getJoinedMesh( .getLink(linkIx) .visualAttachments_; for (auto& visualAttachment : visualAttachments) { - auto objectTransform = Magnum::EigenIntegration::cast< - Eigen::Transform>( - visualAttachment.first->absoluteTransformationMatrix()); + auto objectTransform = + visualAttachment.first->absoluteTransformationMatrix(); std::string meshHandle = visualAttachment.second; meshComponentStates[meshHandle].push_back(objectTransform); } @@ -931,7 +928,8 @@ assets::MeshData::ptr Simulator::getJoinedMesh( } joinedMesh->vbo.reserve(joinedObjectMesh->vbo.size() + prevNumVerts); for (auto& vert : joinedObjectMesh->vbo) { - joinedMesh->vbo.push_back(meshTransform * vert); + auto newVert = meshTransform.transformPoint(vert); + joinedMesh->vbo.push_back(newVert); } } } diff --git a/src/tests/CullingTest.cpp b/src/tests/CullingTest.cpp index cc9f10eb58..ed2e1b91c3 100644 --- a/src/tests/CullingTest.cpp +++ b/src/tests/CullingTest.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include diff --git a/src/tests/GeoTest.cpp b/src/tests/GeoTest.cpp index c3577d2f09..56f8a784cd 100644 --- a/src/tests/GeoTest.cpp +++ b/src/tests/GeoTest.cpp @@ -6,8 +6,8 @@ #include #include #include -#include #include +#include #include "esp/core/Utility.h" #include "esp/geo/CoordinateFrame.h" #include "esp/geo/Geo.h" @@ -124,80 +124,84 @@ void GeoTest::aabb() { void GeoTest::obbConstruction() { OBB obb1; - const vec3f center(0, 0, 0); - const vec3f dimensions(20, 2, 10); - const quatf rot1 = quatf::FromTwoVectors(vec3f::UnitY(), vec3f::UnitZ()); + const Mn::Vector3 center(0, 0, 0); + const Mn::Vector3 dimensions(20, 2, 10); + const auto rot1 = + Mn::Quaternion::rotation(Mn::Vector3::yAxis(), Mn::Vector3::zAxis()); OBB obb2(center, dimensions, rot1); - CORRADE_VERIFY(obb2.center().isApprox(center)); - CORRADE_VERIFY(obb2.sizes().isApprox(dimensions)); - CORRADE_VERIFY(obb2.halfExtents().isApprox(dimensions / 2)); - CORRADE_VERIFY(obb2.rotation().coeffs().isApprox(rot1.coeffs())); + CORRADE_COMPARE(obb2.center(), center); + CORRADE_COMPARE(obb2.sizes(), dimensions); + CORRADE_COMPARE(obb2.halfExtents(), dimensions / 2); + CORRADE_COMPARE(obb2.rotation(), rot1); - box3f aabb(vec3f(-1, -2, -3), vec3f(3, 2, 1)); + Mn::Range3D aabb(Mn::Vector3(-1, -2, -3), Mn::Vector3(3, 2, 1)); OBB obb3(aabb); - CORRADE_VERIFY(obb3.center().isApprox(aabb.center())); - CORRADE_VERIFY(obb3.toAABB().isApprox(aabb)); + CORRADE_COMPARE(obb3.center(), aabb.center()); + CORRADE_COMPARE(obb3.toAABB(), aabb); } void GeoTest::obbFunctions() { - const vec3f center(0, 0, 0); - const vec3f dimensions(20, 2, 10); - const quatf rot1 = quatf::FromTwoVectors(vec3f::UnitY(), vec3f::UnitZ()); + const Mn::Vector3 center(0, 0, 0); + const Mn::Vector3 dimensions(20, 2, 10); + const auto rot1 = + Mn::Quaternion::rotation(Mn::Vector3::yAxis(), Mn::Vector3::zAxis()); OBB obb2(center, dimensions, rot1); - CORRADE_VERIFY(obb2.contains(vec3f(0, 0, 0))); - CORRADE_VERIFY(obb2.contains(vec3f(-5, -2, 0.5))); - CORRADE_VERIFY(obb2.contains(vec3f(5, 0, -0.5))); - CORRADE_VERIFY(!obb2.contains(vec3f(5, 0, 2))); - CORRADE_VERIFY(!obb2.contains(vec3f(-10, 0.5, -2))); - - const box3f aabb = obb2.toAABB(); - - CORRADE_COMPARE(Mn::Vector3{aabb.min()}, (Mn::Vector3{-10.0f, -5.0f, -1.0f})); - CORRADE_COMPARE(Mn::Vector3{aabb.max()}, (Mn::Vector3{10.0f, 5.0f, 1.0f})); - - const Transform identity = obb2.worldToLocal() * obb2.localToWorld(); - CORRADE_VERIFY(identity.isApprox(Transform::Identity())); - CORRADE_VERIFY(obb2.contains(obb2.localToWorld() * vec3f(0, 0, 0))); - CORRADE_VERIFY(obb2.contains(obb2.localToWorld() * vec3f(1, -1, 1))); - CORRADE_VERIFY(obb2.contains(obb2.localToWorld() * vec3f(-1, -1, -1))); - CORRADE_VERIFY(!obb2.contains(obb2.localToWorld() * vec3f(-1, -1.5, -1))); - CORRADE_COMPARE_AS(obb2.distance(vec3f(-20, 0, 0)), 10, float); - CORRADE_COMPARE_AS(obb2.distance(vec3f(-10, -5, 2)), 1, float); + CORRADE_VERIFY(obb2.contains(Mn::Vector3(0, 0, 0))); + CORRADE_VERIFY(obb2.contains(Mn::Vector3(-5, -2, 0.5))); + CORRADE_VERIFY(obb2.contains(Mn::Vector3(5, 0, -0.5))); + CORRADE_VERIFY(!obb2.contains(Mn::Vector3(5, 0, 2))); + CORRADE_VERIFY(!obb2.contains(Mn::Vector3(-10, 0.5, -2))); + + const auto aabb = obb2.toAABB(); + + CORRADE_COMPARE(aabb.min(), (Mn::Vector3{-10.0f, -5.0f, -1.0f})); + CORRADE_COMPARE(aabb.max(), (Mn::Vector3{10.0f, 5.0f, 1.0f})); + + const auto identity = obb2.worldToLocal() * obb2.localToWorld(); + CORRADE_COMPARE(identity, Mn::Matrix4()); + CORRADE_VERIFY( + obb2.contains(obb2.localToWorld().transformPoint(Mn::Vector3(0, 0, 0)))); + CORRADE_VERIFY( + obb2.contains(obb2.localToWorld().transformPoint(Mn::Vector3(1, -1, 1)))); + CORRADE_VERIFY(obb2.contains( + obb2.localToWorld().transformPoint(Mn::Vector3(-1, -1, -1)))); + CORRADE_VERIFY(!obb2.contains( + obb2.localToWorld().transformPoint(Mn::Vector3(-1, -1.5, -1)))); + CORRADE_COMPARE_AS(obb2.distance(Mn::Vector3(-20, 0, 0)), 10, float); + CORRADE_COMPARE_AS(obb2.distance(Mn::Vector3(-10, -5, 2)), 1, float); } void GeoTest::coordinateFrame() { - const vec3f origin(1, -2, 3); - const vec3f up(0, 0, 1); - const vec3f front(-1, 0, 0); - quatf rotation = quatf::FromTwoVectors(ESP_UP, up) * - quatf::FromTwoVectors(ESP_FRONT, front); - Transform xform; - xform.rotate(rotation); - xform.translate(origin); + const Mn::Vector3 origin(1, -2, 3); + const Mn::Vector3 up(0, 0, 1); + const Mn::Vector3 front(-1, 0, 0); + auto rotation = Mn::Quaternion::rotation(ESP_UP, up) * + Mn::Quaternion::rotation(ESP_FRONT, front); + Mn::Matrix4 xform = Mn::Matrix4::from(rotation.toMatrix(), origin); CoordinateFrame c1(up, front, origin); - CORRADE_VERIFY(c1.up().isApprox(up)); - CORRADE_VERIFY(c1.gravity().isApprox(-up)); - CORRADE_VERIFY(c1.front().isApprox(front)); - CORRADE_VERIFY(c1.back().isApprox(-front)); - CORRADE_VERIFY(c1.up().isApprox(rotation * ESP_UP)); - CORRADE_VERIFY(c1.front().isApprox(rotation * ESP_FRONT)); - CORRADE_VERIFY(c1.origin().isApprox(origin)); - CORRADE_VERIFY(c1.rotationWorldToFrame().isApprox(rotation)); + CORRADE_COMPARE(c1.up(), up); + CORRADE_COMPARE(c1.gravity(), -up); + CORRADE_COMPARE(c1.front(), front); + CORRADE_COMPARE(c1.back(), -front); + CORRADE_COMPARE(c1.up(), rotation.transformVectorNormalized(ESP_UP)); + CORRADE_COMPARE(c1.front(), rotation.transformVectorNormalized(ESP_FRONT)); + CORRADE_COMPARE(c1.origin(), origin); + CORRADE_COMPARE(c1.rotationWorldToFrame(), rotation); CoordinateFrame c2(rotation, origin); - CORRADE_VERIFY(c1 == c2); - CORRADE_VERIFY(c2.up().isApprox(up)); - CORRADE_VERIFY(c2.gravity().isApprox(-up)); - CORRADE_VERIFY(c2.front().isApprox(front)); - CORRADE_VERIFY(c2.back().isApprox(-front)); - CORRADE_VERIFY(c2.up().isApprox(rotation * ESP_UP)); - CORRADE_VERIFY(c2.front().isApprox(rotation * ESP_FRONT)); - CORRADE_VERIFY(c2.origin().isApprox(origin)); - CORRADE_VERIFY(c2.rotationWorldToFrame().isApprox(rotation)); - - const std::string j = R"({"up":[0,0,1],"front":[-1,0,0],"origin":[1,-2,3]})"; + CORRADE_COMPARE(c1, c2); + CORRADE_COMPARE(c2.up(), up); + CORRADE_COMPARE(c2.gravity(), -up); + CORRADE_COMPARE(c2.front(), front); + CORRADE_COMPARE(c2.back(), -front); + CORRADE_COMPARE(c2.up(), rotation.transformVectorNormalized(ESP_UP)); + CORRADE_COMPARE(c2.front(), rotation.transformVectorNormalized(ESP_FRONT)); + CORRADE_COMPARE(c2.origin(), origin); + CORRADE_COMPARE(c2.rotationWorldToFrame(), rotation); + + const std::string j = R"("up":[0,0,1],"front":[-1,0,0],"origin":[1,-2,3])"; CORRADE_COMPARE(c1.toString(), j); } diff --git a/src/tests/GibsonSceneTest.cpp b/src/tests/GibsonSceneTest.cpp index 67b062cd99..80eaedca6d 100644 --- a/src/tests/GibsonSceneTest.cpp +++ b/src/tests/GibsonSceneTest.cpp @@ -11,6 +11,7 @@ #include "configure.h" namespace Cr = Corrade; +namespace Mn = Magnum; using esp::scene::SemanticScene; using esp::sim::Simulator; @@ -49,10 +50,10 @@ void GibsonSceneTest::testGibsonScene() { CORRADE_VERIFY(semanticScene.objects().size() != 2); auto object = semanticScene.objects()[1]; CORRADE_COMPARE(object->category()->name(""), "microwave"); - CORRADE_VERIFY( - object->obb().center().isApprox(esp::vec3f(2.83999, 4.76085, 1.49223))); - CORRADE_VERIFY( - object->obb().sizes().isApprox(esp::vec3f(0.406775, 1.28023, 0.454744))); + CORRADE_COMPARE(object->obb().center(), + Mn::Vector3(2.83999, 4.76085, 1.49223)); + CORRADE_COMPARE(object->obb().sizes(), + Mn::Vector3(0.406775, 1.28023, 0.454744)); object = semanticScene.objects()[2]; CORRADE_COMPARE(object->category()->name(""), "oven"); object = semanticScene.objects()[3]; @@ -74,10 +75,10 @@ void GibsonSceneTest::testGibsonSemanticScene() { CORRADE_COMPARE(semanticScene->objects().size(), 34); const auto& microwave = semanticScene->objects()[1]; CORRADE_COMPARE(microwave->category()->name(""), "microwave"); - CORRADE_VERIFY(microwave->obb().center().isApprox( - esp::vec3f(2.83999, 4.76085, 1.49223))); - CORRADE_VERIFY(microwave->obb().sizes().isApprox( - esp::vec3f(0.406775, 1.28023, 0.454744))); + CORRADE_COMPARE(microwave->obb().center(), + Mn::Vector3(2.83999, 4.76085, 1.49223)); + CORRADE_COMPARE(microwave->obb().sizes(), + Mn::Vector3(0.406775, 1.28023, 0.454744)); } } // namespace diff --git a/src/tests/IOTest.cpp b/src/tests/IOTest.cpp index 100ee1ee3c..21c3e5c599 100644 --- a/src/tests/IOTest.cpp +++ b/src/tests/IOTest.cpp @@ -408,12 +408,12 @@ void IOTest::testJsonEspTypes() { { // vec3f - _testJsonReadWrite(esp::vec3f{1, 2, 3}, "myvec3f", d); + _testJsonReadWrite(Mn::Vector3{1, 2, 3}, "myvec3f", d); // test reading the wrong type (wrong number of fields) std::vector wrongNumFieldsVec{1, 3, 4, 4}; esp::io::addMember(d, "mywrongNumFieldsVec", wrongNumFieldsVec, allocator); - esp::vec3f vec3; + Mn::Vector3 vec3; CORRADE_VERIFY(!esp::io::readMember(d, "mywrongNumFieldsVec", vec3)); // test reading the wrong type (array elements aren't numbers) @@ -442,9 +442,9 @@ void IOTest::testJsonEspTypes() { esp::assets::AssetInfo assetInfo{ AssetType::Mp3dMesh, "test_filepath2", - esp::geo::CoordinateFrame(esp::vec3f(1.f, 0.f, 0.f), - esp::vec3f(0.f, 0.f, 1.f), - esp::vec3f(1.f, 2.f, 3.f)), + esp::geo::CoordinateFrame(Mn::Vector3(1.f, 0.f, 0.f), + Mn::Vector3(0.f, 0.f, 1.f), + Mn::Vector3(1.f, 2.f, 3.f)), 4.f, true, false}; @@ -454,10 +454,9 @@ void IOTest::testJsonEspTypes() { CORRADE_VERIFY(assetInfo2.type == assetInfo.type); CORRADE_COMPARE(assetInfo2.filepath, assetInfo.filepath); - CORRADE_VERIFY(assetInfo2.frame.up().isApprox(assetInfo.frame.up())); - CORRADE_VERIFY(assetInfo2.frame.front().isApprox(assetInfo.frame.front())); - CORRADE_VERIFY( - assetInfo2.frame.origin().isApprox(assetInfo.frame.origin())); + CORRADE_COMPARE(assetInfo2.frame.up(), (assetInfo.frame.up())); + CORRADE_COMPARE(assetInfo2.frame.front(), (assetInfo.frame.front())); + CORRADE_COMPARE(assetInfo2.frame.origin(), (assetInfo.frame.origin())); CORRADE_COMPARE(assetInfo2.virtualUnitToMeters, assetInfo.virtualUnitToMeters); CORRADE_COMPARE(assetInfo2.forceFlatShading, assetInfo.forceFlatShading); diff --git a/src/tests/Mp3dTest.cpp b/src/tests/Mp3dTest.cpp index 44223877d7..02a12badef 100644 --- a/src/tests/Mp3dTest.cpp +++ b/src/tests/Mp3dTest.cpp @@ -39,12 +39,12 @@ void Mp3dTest::testLoad() { mp3dAttr->setSemanticDescriptorFullPath(filename); esp::scene::SemanticScene house; - const esp::quatf alignGravity = - esp::quatf::FromTwoVectors(-esp::vec3f::UnitZ(), esp::geo::ESP_GRAVITY); - const esp::quatf alignFront = - esp::quatf::FromTwoVectors(-esp::vec3f::UnitX(), esp::geo::ESP_FRONT); - esp::scene::SemanticScene::loadSemanticSceneDescriptor( - mp3dAttr, house, alignFront * alignGravity); + const auto alignGravity = + Mn::Quaternion::rotation(esp::geo::ESP_FRONT, esp::geo::ESP_GRAVITY); + const auto alignFront = + Mn::Quaternion::rotation(-Mn::Vector3::xAxis(), esp::geo::ESP_FRONT); + esp::scene::SemanticScene::loadMp3dHouse(filename, house, + alignFront * alignGravity); ESP_DEBUG() << "House{nobjects:" << house.count("objects") << ",nlevels:" << house.count("levels") << ",nregions:" << house.count("regions") diff --git a/src/tests/NavTest.cpp b/src/tests/NavTest.cpp index 3c7280a0b0..b0dc5829e0 100644 --- a/src/tests/NavTest.cpp +++ b/src/tests/NavTest.cpp @@ -15,11 +15,12 @@ #include "configure.h" namespace Cr = Corrade; +namespace Mn = Magnum; namespace { -void printPathPoint(int run, int step, const esp::vec3f& p, float distance) { - ESP_VERY_VERBOSE() << run << "," << step << "," << Magnum::Vector3{p} << "," +void printPathPoint(int run, int step, const Mn::Vector3& p, float distance) { + ESP_VERY_VERBOSE() << run << "," << step << "," << Mn::Vector3{p} << "," << distance; } @@ -59,11 +60,10 @@ void NavTest::PathFinderLoadTest() { CORRADE_COMPARE(pf.islandRadius(path.points[j]), islandSize); } CORRADE_COMPARE(pf.islandRadius(path.requestedEnd), islandSize); - const esp::vec3f& pathStart = path.points.front(); - const esp::vec3f& pathEnd = path.points.back(); - const esp::vec3f end = pf.tryStep(pathStart, pathEnd); - ESP_DEBUG() << "tryStep initial end=" << pathEnd.transpose() - << ", final end=" << end.transpose(); + const Mn::Vector3& pathStart = path.points.front(); + const Mn::Vector3& pathEnd = path.points.back(); + const Mn::Vector3 end = pf.tryStep(pathStart, pathEnd); + ESP_DEBUG() << "tryStep initial end=" << pathEnd << ", final end=" << end; CORRADE_COMPARE_AS(path.geodesicDistance, std::numeric_limits::infinity(), Cr::TestSuite::Compare::Less); @@ -79,8 +79,9 @@ void printRandomizedPathSet(esp::nav::PathFinder& pf) { ESP_VERY_VERBOSE() << "run,step,x,y,z,geodesicDistance"; for (int i = 0; i < 100; ++i) { const float r = 0.1; - esp::vec3f rv(random.uniform_float(-r, r), 0, random.uniform_float(-r, r)); - esp::vec3f rv2(random.uniform_float(-r, r), 0, random.uniform_float(-r, r)); + Mn::Vector3 rv(random.uniform_float(-r, r), 0, random.uniform_float(-r, r)); + Mn::Vector3 rv2(random.uniform_float(-r, r), 0, + random.uniform_float(-r, r)); path.requestedStart += rv; path.requestedEnd += rv2; const bool foundPath = pf.findPath(path); @@ -94,8 +95,7 @@ void printRandomizedPathSet(esp::nav::PathFinder& pf) { path.geodesicDistance); } else { ESP_WARNING() << "Failed to find shortest path between start=" - << path.requestedStart.transpose() - << "and end=" << path.requestedEnd.transpose(); + << path.requestedStart << "and end=" << path.requestedEnd; } } } @@ -105,8 +105,8 @@ void NavTest::PathFinderTestCases() { pf.loadNavMesh(Cr::Utility::Path::join( SCENE_DATASETS, "habitat-test-scenes/skokloster-castle.navmesh")); esp::nav::ShortestPath testPath; - testPath.requestedStart = esp::vec3f(-6.493, 0.072, -3.292); - testPath.requestedEnd = esp::vec3f(-8.98, 0.072, -0.62); + testPath.requestedStart = Mn::Vector3(-6.493, 0.072, -3.292); + testPath.requestedEnd = Mn::Vector3(-8.98, 0.072, -0.62); pf.findPath(testPath); CORRADE_COMPARE(testPath.points.size(), 0); CORRADE_COMPARE(testPath.geodesicDistance, @@ -114,14 +114,15 @@ void NavTest::PathFinderTestCases() { testPath.requestedStart = pf.getRandomNavigablePoint(); // Jitter the point just enough so that it isn't exactly the same - testPath.requestedEnd = testPath.requestedStart + esp::vec3f(0.01, 0.0, 0.01); + testPath.requestedEnd = + testPath.requestedStart + Mn::Vector3(0.01, 0.0, 0.01); pf.findPath(testPath); // There should be 2 points CORRADE_COMPARE(testPath.points.size(), 2); // The geodesicDistance should be almost exactly the L2 dist CORRADE_COMPARE_AS( std::abs(testPath.geodesicDistance - - (testPath.requestedStart - testPath.requestedEnd).norm()), + (testPath.requestedStart - testPath.requestedEnd).length()), 0.001, Cr::TestSuite::Compare::LessOrEqual); } @@ -130,12 +131,12 @@ void NavTest::PathFinderTestNonNavigable() { pf.loadNavMesh(Cr::Utility::Path::join( SCENE_DATASETS, "habitat-test-scenes/skokloster-castle.navmesh")); - const esp::vec3f nonNavigablePoint{1e2, 1e2, 1e2}; + const Mn::Vector3 nonNavigablePoint{1e2, 1e2, 1e2}; - const esp::vec3f resultPoint = pf.tryStep( - nonNavigablePoint, nonNavigablePoint + esp::vec3f{0.25, 0, 0}); + const Mn::Vector3 resultPoint = pf.tryStep( + nonNavigablePoint, nonNavigablePoint + Mn::Vector3{0.25, 0, 0}); - CORRADE_VERIFY(nonNavigablePoint.isApprox(resultPoint)); + CORRADE_VERIFY(nonNavigablePoint == resultPoint); } void NavTest::PathFinderTestSeed() { @@ -145,23 +146,23 @@ void NavTest::PathFinderTestSeed() { // The same seed should produce the same point pf.seed(1); - esp::vec3f firstPoint = pf.getRandomNavigablePoint(); + Mn::Vector3 firstPoint = pf.getRandomNavigablePoint(); pf.seed(1); - esp::vec3f secondPoint = pf.getRandomNavigablePoint(); + Mn::Vector3 secondPoint = pf.getRandomNavigablePoint(); CORRADE_COMPARE(firstPoint, secondPoint); // Different seeds should produce different points pf.seed(2); - esp::vec3f firstPoint2 = pf.getRandomNavigablePoint(); + Mn::Vector3 firstPoint2 = pf.getRandomNavigablePoint(); pf.seed(3); - esp::vec3f secondPoint2 = pf.getRandomNavigablePoint(); + Mn::Vector3 secondPoint2 = pf.getRandomNavigablePoint(); CORRADE_COMPARE_AS(firstPoint2, secondPoint2, Cr::TestSuite::Compare::NotEqual); // One seed should produce different points when sampled twice pf.seed(4); - esp::vec3f firstPoint3 = pf.getRandomNavigablePoint(); - esp::vec3f secondPoint3 = pf.getRandomNavigablePoint(); + Mn::Vector3 firstPoint3 = pf.getRandomNavigablePoint(); + Mn::Vector3 secondPoint3 = pf.getRandomNavigablePoint(); CORRADE_COMPARE_AS(firstPoint3, secondPoint3, Cr::TestSuite::Compare::NotEqual); } diff --git a/src/tests/PathFinderTest.cpp b/src/tests/PathFinderTest.cpp index 8d153d6ce0..5f5165dcfb 100644 --- a/src/tests/PathFinderTest.cpp +++ b/src/tests/PathFinderTest.cpp @@ -9,7 +9,6 @@ #include #include -#include #include #include #include @@ -66,7 +65,7 @@ void PathFinderTest::bounds() { Mn::Vector3 minExpected{-9.75916f, -0.390081f, 0.973853f}; Mn::Vector3 maxExpected{8.56903f, 6.43441f, 25.5983f}; - std::pair bounds = pathFinder.bounds(); + std::pair bounds = pathFinder.bounds(); CORRADE_COMPARE(Mn::Vector3{bounds.first}, minExpected); CORRADE_COMPARE(Mn::Vector3{bounds.second}, maxExpected); @@ -84,8 +83,7 @@ void PathFinderTest::tryStepNoSliding() { for (int j = 0; j < 10; ++j) { Mn::Vector3 targetPos = pos + stepDir; Mn::Vector3 actualEnd = pathFinder.tryStepNoSliding(pos, targetPos); - CORRADE_VERIFY(pathFinder.isNavigable( - Mn::EigenIntegration::cast(actualEnd))); + CORRADE_VERIFY(pathFinder.isNavigable(actualEnd)); // The test becomes unreliable if we moved a very small distance if (Mn::Math::gather<'x', 'z'>(actualEnd - pos).dot() < 1e-5) @@ -108,7 +106,7 @@ void PathFinderTest::multiGoalPath() { pathFinder.seed(0); for (int __j = 0; __j < 1000; ++__j) { - std::vector points; + std::vector points; points.reserve(10); for (int i = 0; i < 10; ++i) { points.emplace_back(pathFinder.getRandomNavigablePoint()); @@ -142,7 +140,7 @@ void PathFinderTest::testCaching() { esp::nav::MultiGoalShortestPath cachePath; { - std::vector rqEnds; + std::vector rqEnds; rqEnds.reserve(25); for (int i = 0; i < 25; ++i) { rqEnds.emplace_back(pathFinder.getRandomNavigablePoint()); @@ -195,7 +193,7 @@ void PathFinderTest::benchmarkMultiGoal() { path.requestedStart = pathFinder.getRandomNavigablePoint(); } while (pathFinder.islandRadius(path.requestedStart) < 10.0); - std::vector rqEnds; + std::vector rqEnds; rqEnds.reserve(1000); for (int i = 0; i < 1000; ++i) { rqEnds.emplace_back(pathFinder.getRandomNavigablePoint()); diff --git a/src/tests/ReplicaSceneTest.cpp b/src/tests/ReplicaSceneTest.cpp index 642efcef16..aa79066b9e 100644 --- a/src/tests/ReplicaSceneTest.cpp +++ b/src/tests/ReplicaSceneTest.cpp @@ -6,8 +6,6 @@ #include #include -#include -#include #include #include @@ -115,8 +113,7 @@ void ReplicaSceneTest::testSemanticSceneOBB() { quadCenter += vbo[ibo[fid + i]] / 6; } - CORRADE_VERIFY( - obj->obb().contains(esp::vec3f{quadCenter.data()}, 5e-2)); + CORRADE_VERIFY(obj->obb().contains(quadCenter, 5e-2)); } } } @@ -143,24 +140,24 @@ void ReplicaSceneTest::testSemanticSceneLoading() { CORRADE_VERIFY(obj12); CORRADE_COMPARE(obj12->id(), "_12"); // obj12's obb - // Eigen Calc // center:[3.52103,-1.00543,-1.02705] // halfextents:[0.169882,0.160166,0.01264] // rotational quat coefficients:[-0.70592,0.0131598,0.0157815,0.707994] // Old Sophus calc: // {c:[3.52103,-1.00543,-1.02705],h:[0.169882,0.160166,0.01264],r:[-0.70592,0.0131598,0.0157815,0.707994]} - CORRADE_VERIFY( - obj12->obb().center().isApprox(esp::vec3f{3.52103, -1.00543, -1.02705})); - CORRADE_VERIFY(obj12->obb().halfExtents().isApprox( - esp::vec3f{0.169882, 0.160166, 0.01264})); - CORRADE_VERIFY(obj12->obb().rotation().coeffs().isApprox( - esp::vec4f{-0.70592, 0.0131598, 0.0157815, 0.707994})); + CORRADE_COMPARE(obj12->obb().center(), + Magnum::Vector3(3.52103, -1.00543, -1.02705)); + CORRADE_COMPARE(obj12->obb().halfExtents(), + Magnum::Vector3(0.169882, 0.160166, 0.01264)); + CORRADE_COMPARE( + obj12->obb().rotation(), + Magnum::Quaternion({-0.70592, 0.0131598, 0.0157815}, 0.707994)); CORRADE_VERIFY(obj12->category()); CORRADE_COMPARE(obj12->category()->index(), 13); CORRADE_COMPARE(obj12->category()->name(), "book"); -} +} // ReplicaSceneTest::testSemanticSceneLoading void ReplicaSceneTest::testSemanticSceneDescriptorReplicaCAD() { if (!Cr::Utility::Path::exists(replicaCAD)) { diff --git a/src/tests/ResourceManagerTest.cpp b/src/tests/ResourceManagerTest.cpp index fec1e21fdc..c6d8e71fc1 100644 --- a/src/tests/ResourceManagerTest.cpp +++ b/src/tests/ResourceManagerTest.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include diff --git a/src/tests/SimTest.cpp b/src/tests/SimTest.cpp index ffaf2531b3..8285e47b8b 100644 --- a/src/tests/SimTest.cpp +++ b/src/tests/SimTest.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -63,6 +62,12 @@ const std::string physicsConfigFile = Cr::Utility::Path::join(TEST_ASSETS, "testing.physics_config.json"); const std::string screenshotDir = Cr::Utility::Path::join(TEST_ASSETS, "screenshots/"); +const std::string nestedBoxPath = + Cr::Utility::Path::join(TEST_ASSETS, "objects/nested_box"); + +const std::string nestedBoxConfigPath = + Cr::Utility::Path::join(TEST_ASSETS, + "objects/nested_box.object_config.json"); struct SimTest : Cr::TestSuite::Tester { explicit SimTest(); @@ -88,8 +93,7 @@ struct SimTest : Cr::TestSuite::Tester { auto sim = Simulator::create_unique(simConfig); auto objAttrMgr = sim->getObjectAttributesManager(); - objAttrMgr->loadAllJSONConfigsFromPath( - Cr::Utility::Path::join(TEST_ASSETS, "objects/nested_box"), true); + objAttrMgr->loadAllJSONConfigsFromPath(nestedBoxPath, true); sim->setLightSetup(self.lightSetup1, "custom_lighting_1"); sim->setLightSetup(self.lightSetup2, "custom_lighting_2"); @@ -118,8 +122,7 @@ struct SimTest : Cr::TestSuite::Tester { MetadataMediator::ptr MM = MetadataMediator::create(simConfig); auto sim = Simulator::create_unique(simConfig, MM); auto objAttrMgr = sim->getObjectAttributesManager(); - objAttrMgr->loadAllJSONConfigsFromPath( - Cr::Utility::Path::join(TEST_ASSETS, "objects/nested_box"), true); + objAttrMgr->loadAllJSONConfigsFromPath(nestedBoxPath, true); sim->setLightSetup(self.lightSetup1, "custom_lighting_1"); sim->setLightSetup(self.lightSetup2, "custom_lighting_2"); @@ -454,7 +457,7 @@ void SimTest::recomputeNavmeshWithStaticObjects() { simulator->recomputeNavMesh(*simulator->getPathFinder().get(), navMeshSettings); - esp::vec3f randomNavPoint = + Mn::Vector3 randomNavPoint = simulator->getPathFinder()->getRandomNavigablePoint(); while (simulator->getPathFinder()->distanceToClosestObstacle(randomNavPoint) < 1.0 || @@ -493,7 +496,7 @@ void SimTest::recomputeNavmeshWithStaticObjects() { obj->setTranslation(Magnum::Vector3{randomNavPoint}); obj->setTranslation(obj->getTranslation() + Magnum::Vector3{0, 0.5, 0}); obj->setMotionType(esp::physics::MotionType::STATIC); - esp::vec3f offset(0.75, 0, 0); + Mn::Vector3 offset(0.75, 0, 0); CORRADE_VERIFY(simulator->getPathFinder()->isNavigable(randomNavPoint, 0.1)); CORRADE_VERIFY( simulator->getPathFinder()->isNavigable(randomNavPoint + offset, 0.2)); @@ -637,8 +640,7 @@ void SimTest::addObjectByHandle() { CORRADE_COMPARE(obj, nullptr); // pass valid object_config.json filepath as handle to addObjectByHandle - const auto validHandle = Cr::Utility::Path::join( - TEST_ASSETS, "objects/nested_box.object_config.json"); + const auto validHandle = nestedBoxConfigPath; obj = rigidObjMgr->addObjectByHandle(validHandle); CORRADE_VERIFY(obj->isAlive()); CORRADE_VERIFY(obj->getID() != esp::ID_UNDEFINED); @@ -685,8 +687,7 @@ void SimTest::addObjectInvertedScale() { simulator->addSensorToObject(esp::RIGID_STAGE_ID, pinholeCameraSpec); // Add 2 objects and take initial non-negative scaled observation - const auto objHandle = Cr::Utility::Path::join( - TEST_ASSETS, "objects/nested_box.object_config.json"); + const auto objHandle = nestedBoxConfigPath; Observation expectedObservation; addObjectsAndMakeObservation(*simulator, *pinholeCameraSpec, objHandle, @@ -796,7 +797,7 @@ void SimTest::addSensorToObject() { CORRADE_VERIFY(cameraSensor.getObservation(*simulator, observation)); CORRADE_VERIFY(cameraSensor.getObservationSpace(obsSpace)); - esp::vec2i defaultResolution = {128, 128}; + Mn::Vector2i defaultResolution = {128, 128}; std::vector expectedShape{{static_cast(defaultResolution[0]), static_cast(defaultResolution[1]), 4}}; diff --git a/src/utils/viewer/viewer.cpp b/src/utils/viewer/viewer.cpp index db94fec6a8..328e927112 100644 --- a/src/utils/viewer/viewer.cpp +++ b/src/utils/viewer/viewer.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -414,20 +413,21 @@ Key Commands: // single inline for logging agent state msgs, so can be easily modified inline void showAgentStateMsg(bool showPos, bool showOrient) { - std::stringstream strDat(""); + std::string res(""); if (showPos) { - strDat << "Agent position " - << Eigen::Map(agentBodyNode_->translation().data()) - << " "; + auto v = agentBodyNode_->translation(); + Cr::Utility::formatInto(res, res.length(), + "Agent position : [{} {} {}] ", v.x(), v.y(), + v.z()); } if (showOrient) { - strDat << "Agent orientation " - << esp::quatf(agentBodyNode_->rotation()).coeffs().transpose(); + Mn::Quaternion q = agentBodyNode_->rotation(); + auto qv = q.vector(); + Cr::Utility::formatInto(res, res.length(), ": w:{} [{} {} {}]", + q.scalar(), qv.x(), qv.y(), qv.z()); } - - auto str = strDat.str(); - if (str.size() > 0) { - ESP_DEBUG() << str; + if (res.size() > 0) { + ESP_DEBUG() << res; } } @@ -1127,7 +1127,7 @@ void saveTransformToFile(const std::string& filename, file << t[i] << " "; } ESP_DEBUG() << "Transformation matrix saved to" << filename << ":" - << Eigen::Map(transform.data()); + << transform; }; save(agentTransform); save(sensorTransform); @@ -1176,7 +1176,7 @@ bool loadTransformFromFile(const std::string& filename, } transform = temp; ESP_DEBUG() << "Transformation matrix loaded from" << filename << ":" - << Eigen::Map(transform.data()); + << transform; return true; }; // NOTE: load Agent first!! @@ -2237,7 +2237,7 @@ void Viewer::keyPressEvent(KeyEvent& event) { break; case Key::Nine: if (simulator_->getPathFinder()->isLoaded()) { - const esp::vec3f position = + const Mn::Vector3 position = simulator_->getPathFinder()->getRandomNavigablePoint(); agentBodyNode_->setTranslation(Mn::Vector3(position)); } diff --git a/src_python/habitat_sim/agent/agent.py b/src_python/habitat_sim/agent/agent.py index 4cb2bbc037..f7bc603414 100755 --- a/src_python/habitat_sim/agent/agent.py +++ b/src_python/habitat_sim/agent/agent.py @@ -294,7 +294,7 @@ def state(self): return self.get_state() @state.setter - def state(self, new_state): + def state(self, new_state: AgentState): self.set_state( new_state, reset_sensors=True, infer_sensor_states=True, is_initial=False ) diff --git a/src_python/habitat_sim/geo.py b/src_python/habitat_sim/geo.py index d7005188b3..ee912e3072 100644 --- a/src_python/habitat_sim/geo.py +++ b/src_python/habitat_sim/geo.py @@ -6,7 +6,7 @@ Encapsulates global geometry utilities. """ -from habitat_sim._ext.habitat_sim_bindings import OBB, BBox, Ray +from habitat_sim._ext.habitat_sim_bindings import OBB, Ray from habitat_sim._ext.habitat_sim_bindings.geo import ( BACK, FRONT, @@ -20,7 +20,6 @@ ) __all__ = [ - "BBox", "OBB", "UP", "GRAVITY", diff --git a/src_python/habitat_sim/utils/common.py b/src_python/habitat_sim/utils/common.py index f6f309a0f5..2e9e43b04b 100755 --- a/src_python/habitat_sim/utils/common.py +++ b/src_python/habitat_sim/utils/common.py @@ -122,16 +122,15 @@ def quat_from_two_vectors(v0: np.ndarray, v1: np.ndarray) -> qt.quaternion: return qt.quaternion(s * 0.5, *(axis / s)) -def angle_between_quats(q1: qt.quaternion, q2: qt.quaternion) -> float: - r"""Computes the angular distance between two quaternions +def angle_between_quats(q1: mn.Quaternion, q2: mn.Quaternion) -> float: + r"""Computes the angular distance between two magnum quaternions :return: The angular distance between q1 and q2 in radians """ - q1_inv = np.conjugate(q1) - dq = q1_inv * q2 + dq = q1.inverted() * q2 - return 2 * np.arctan2(np.linalg.norm(dq.imag), np.abs(dq.real)) + return 2 * np.arctan2(dq.vector.length(), np.abs(dq.scalar)) def quat_rotate_vector(q: qt.quaternion, v: np.ndarray) -> np.ndarray: diff --git a/tests/test_agent.py b/tests/test_agent.py index c6812070ed..c8cd334205 100644 --- a/tests/test_agent.py +++ b/tests/test_agent.py @@ -10,12 +10,20 @@ import habitat_sim import habitat_sim.errors -from habitat_sim.utils.common import angle_between_quats, quat_from_angle_axis +from habitat_sim.utils.common import ( + angle_between_quats, + quat_from_angle_axis, + quat_to_magnum, +) def _check_state_same(s1, s2): assert np.allclose(s1.position, s2.position, atol=1e-5) - assert angle_between_quats(s1.rotation, s2.rotation) < 1e-5 + # remove quat_to_magnum once AgentState is refactored to use magnum quaternions + assert ( + angle_between_quats(quat_to_magnum(s1.rotation), quat_to_magnum(s2.rotation)) + < 1e-5 + ) def test_reconfigure(): diff --git a/tests/test_controls.py b/tests/test_controls.py index 1582afc855..1a3fff4234 100644 --- a/tests/test_controls.py +++ b/tests/test_controls.py @@ -9,12 +9,11 @@ import magnum as mn import numpy as np import pytest -import quaternion as qt from hypothesis import strategies as st import habitat_sim import habitat_sim.errors -from habitat_sim.utils.common import angle_between_quats, quat_from_angle_axis +from habitat_sim.utils.common import angle_between_quats, quat_to_magnum def test_no_action(): @@ -48,7 +47,7 @@ def test_no_move_fun(): @attr.s(auto_attribs=True, cmp=False) class ExpectedDelta: delta_pos: np.ndarray = attr.Factory(lambda: np.array([0, 0, 0])) - delta_rot: qt.quaternion = attr.Factory(lambda: qt.quaternion(1, 0, 0, 0)) + delta_rot: mn.Quaternion = attr.Factory(lambda: mn.Quaternion.identity_init()) def _check_state_same(s1, s2): @@ -59,7 +58,7 @@ def _check_state_same(s1, s2): def _check_state_expected(s1, s2, expected: ExpectedDelta): assert np.linalg.norm(s2.position - s1.position - expected.delta_pos) < 1e-5 assert ( - angle_between_quats(s2.rotation * expected.delta_rot.inverse(), s1.rotation) + angle_between_quats(s2.rotation * expected.delta_rot.inverted(), s1.rotation) < 1e-5 ) @@ -72,20 +71,24 @@ def _check_state_expected(s1, s2, expected: ExpectedDelta): ( "turn_right", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(10.0), habitat_sim.geo.GRAVITY) + delta_rot=mn.Quaternion.rotation( + mn.Rad(mn.math.pi / 18.0), habitat_sim.geo.GRAVITY + ) ), ), ( "turn_left", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(10.0), habitat_sim.geo.UP) + delta_rot=mn.Quaternion.rotation( + mn.Rad(mn.math.pi / 18.0), habitat_sim.geo.UP + ) ), ), ] @pytest.mark.parametrize("action,expected", default_body_control_testdata) -def test_default_body_contorls(action, expected): +def test_default_body_controls(action, expected): scene_graph = habitat_sim.SceneGraph() agent_config = habitat_sim.AgentConfiguration() agent_config.action_space = dict( @@ -114,6 +117,15 @@ def test_default_body_contorls(action, expected): agent.act(action) new_state = agent.state + print( + f"pre state rot : [{state.rotation.real}, {state.rotation.imag}] | new state rot : [{new_state.rotation.real}, {new_state.rotation.imag}] " + ) + + state.rotation = quat_to_magnum(state.rotation) + new_state.rotation = quat_to_magnum(new_state.rotation) + + print(f"post state : {state} | new state : {new_state} ") + _check_state_expected(state, new_state, expected) for k, v in state.sensor_states.items(): assert k in new_state.sensor_states @@ -126,37 +138,37 @@ def test_default_body_contorls(action, expected): ( "look_right", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(-10.0), habitat_sim.geo.UP) + delta_rot=mn.Quaternion.rotation(mn.Rad(-10.0), habitat_sim.geo.UP) ), ), ( "look_left", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(10.0), habitat_sim.geo.UP) + delta_rot=mn.Quaternion.rotation(mn.Rad(10.0), habitat_sim.geo.UP) ), ), ( "look_up", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(10.0), habitat_sim.geo.RIGHT) + delta_rot=mn.Quaternion.rotation(mn.Rad(10.0), habitat_sim.geo.RIGHT) ), ), ( "look_down", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(-10.0), habitat_sim.geo.RIGHT) + delta_rot=mn.Quaternion.rotation(mn.Rad(-10.0), habitat_sim.geo.RIGHT) ), ), ( "rotate_sensor_clockwise", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(-10.0), habitat_sim.geo.FRONT) + delta_rot=mn.Quaternion.rotation(mn.Rad(-10.0), habitat_sim.geo.FRONT) ), ), ( "rotate_sensor_anti_clockwise", ExpectedDelta( - delta_rot=quat_from_angle_axis(np.deg2rad(10.0), habitat_sim.geo.FRONT) + delta_rot=mn.Quaternion.rotation(mn.Rad(10.0), habitat_sim.geo.FRONT) ), ), ] @@ -195,8 +207,10 @@ def test_default_sensor_contorls(action, expected): agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(), agent_config) state = agent.state + state.rotation = quat_to_magnum(state.rotation) agent.act(action) new_state = agent.state + new_state.rotation = quat_to_magnum(new_state.rotation) _check_state_same(state, new_state) for k, v in state.sensor_states.items(): diff --git a/tests/test_navmesh.py b/tests/test_navmesh.py index 818ccf4b36..ce7150b5e6 100644 --- a/tests/test_navmesh.py +++ b/tests/test_navmesh.py @@ -211,7 +211,7 @@ def test_save_navmesh_settings(agent_radius_mul, tmpdir): "17DRP5sb8fy": { "island_cache": { 0: {"area": 1.1425001621246338, "radius": 1.6779788732528687}, - 1: {"area": 50.89643096923828, "radius": 8.03792953491211}, + 1: {"area": 50.89643096923828, "radius": 8.037928581237793}, }, "num_islands": 2, }, diff --git a/tests/test_physics.py b/tests/test_physics.py index d39d4f0805..e768fa3d8f 100644 --- a/tests/test_physics.py +++ b/tests/test_physics.py @@ -11,18 +11,12 @@ import magnum as mn import numpy as np import pytest -import quaternion import habitat_sim import habitat_sim.bindings import habitat_sim.physics import habitat_sim.utils.settings -from habitat_sim.utils.common import ( - quat_from_angle_axis, - quat_from_magnum, - quat_to_magnum, - random_quaternion, -) +from habitat_sim.utils.common import random_quaternion from utils import simulate @@ -90,12 +84,12 @@ def test_kinematics(): assert np.allclose(cheezit_box.transformation, I) # test get and set rotation - Q = quat_from_angle_axis(np.pi, np.array([0.0, 1.0, 0.0])) - expected = np.eye(4) - expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q) - cheezit_box.rotation = quat_to_magnum(Q) + Q = mn.Quaternion.rotation(mn.Rad(math.pi), [0.0, 1.0, 0.0]) + expected = mn.Matrix4.from_(Q.to_matrix(), [0.0, 0.0, 0.0]) + cheezit_box.rotation = Q assert np.allclose(cheezit_box.transformation, expected) - assert np.allclose(quat_from_magnum(cheezit_box.rotation), Q) + assert np.allclose(cheezit_box.rotation.scalar, Q.scalar) + assert np.allclose(cheezit_box.rotation.vector, Q.vector) # test object removal rigid_obj_mgr.remove_object_by_id(cheezit_box.object_id) @@ -212,12 +206,13 @@ def test_kinematics_no_physics(): assert np.allclose(cheezit_box.transformation, I) # test get and set rotation - Q = quat_from_angle_axis(np.pi, np.array([0.0, 1.0, 0.0])) - expected = np.eye(4) - expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q) - cheezit_box.rotation = quat_to_magnum(Q) + # test get and set rotation + Q = mn.Quaternion.rotation(mn.Rad(math.pi), [0.0, 1.0, 0.0]) + expected = mn.Matrix4.from_(Q.to_matrix(), [0.0, 0.0, 0.0]) + cheezit_box.rotation = Q assert np.allclose(cheezit_box.transformation, expected) - assert np.allclose(quat_from_magnum(cheezit_box.rotation), Q) + assert np.allclose(cheezit_box.rotation.scalar, Q.scalar) + assert np.allclose(cheezit_box.rotation.vector, Q.vector) # test object removal rigid_obj_mgr.remove_object_by_id(cheezit_box.object_id) diff --git a/tests/test_random_seed.py b/tests/test_random_seed.py index 849eb4c687..2c0185feba 100644 --- a/tests/test_random_seed.py +++ b/tests/test_random_seed.py @@ -23,17 +23,17 @@ def test_random_seed(): point1 = sim.pathfinder.get_random_navigable_point() sim.seed(1) point2 = sim.pathfinder.get_random_navigable_point() - assert all(point1 == point2) + assert point1 == point2 # Test that different seeds give different points sim.seed(2) point1 = sim.pathfinder.get_random_navigable_point() sim.seed(3) point2 = sim.pathfinder.get_random_navigable_point() - assert any(point1 != point2) + assert point1 != point2 # Test that the same seed gives different points when sampled twice sim.seed(4) point1 = sim.pathfinder.get_random_navigable_point() point2 = sim.pathfinder.get_random_navigable_point() - assert any(point1 != point2) + assert point1 != point2 diff --git a/tests/test_semantic_scene.py b/tests/test_semantic_scene.py index 726d6d9fbb..8b71554e33 100644 --- a/tests/test_semantic_scene.py +++ b/tests/test_semantic_scene.py @@ -164,7 +164,7 @@ def test_semantic_scene(scene, make_cfg_settings): scene = sim.semantic_scene for obj in scene.objects: obj.aabb - obj.aabb.sizes + obj.aabb.size obj.aabb.center obj.id obj.obb.rotation