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/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/examples/web_apps/webxr_hand_demo/js/vr_demo.js b/examples/web_apps/webxr_hand_demo/js/vr_demo.js index 8af84d60ce..fea855cf2c 100644 --- a/examples/web_apps/webxr_hand_demo/js/vr_demo.js +++ b/examples/web_apps/webxr_hand_demo/js/vr_demo.js @@ -307,8 +307,8 @@ export class VRDemo { handInfo.push({ index: handIndex, - pos: Module.toVec3f(handPos), - rot: Module.toVec4f(handRot), + pos: handPos, + rot: handRot, gripButton: buttonStates[0], spawnButton: buttonStates[1] }); diff --git a/src/esp/agent/Agent.cpp b/src/esp/agent/Agent.cpp index ef2111cce8..67326e187c 100644 --- a/src/esp/agent/Agent.cpp +++ b/src/esp/agent/Agent.cpp @@ -4,23 +4,20 @@ #include "Agent.h" -#include -#include - #include "esp/scene/ObjectControls.h" #include "esp/sensor/Sensor.h" -using Magnum::EigenIntegration::cast; - namespace esp { namespace agent { +namespace Mn = Magnum; + const std::set Agent::BodyActions = {"moveRight", "moveLeft", "moveForward", "moveBackward", "turnLeft", "turnRight"}; Agent::Agent(scene::SceneNode& agentNode, const AgentConfiguration& cfg) - : Magnum::SceneGraph::AbstractFeature3D(agentNode), + : Mn::SceneGraph::AbstractFeature3D(agentNode), configuration_(cfg), controls_(scene::ObjectControls::create()) { agentNode.setType(scene::SceneNodeType::AGENT); @@ -45,9 +42,8 @@ bool Agent::act(const std::string& actionName) { } } return true; - } else { - return false; } + return false; } bool Agent::hasAction(const std::string& actionName) const { @@ -61,20 +57,22 @@ void Agent::reset() { void Agent::getState(const AgentState::ptr& state) const { // TODO this should be done less hackishly - state->position = cast(node().absoluteTransformation().translation()); - state->rotation = quatf(node().rotation()).coeffs(); + state->position = node().absoluteTransformation().translation(); + // doing this for x,y,z,w format of state's rotation. + auto rot = node().rotation(); + state->rotation = Mn::Vector4(rot.vector(), rot.scalar()); // TODO other state members when implemented } void Agent::setState(const AgentState& state, const bool resetSensors /*= true*/) { - node().setTranslation(Magnum::Vector3(state.position)); + node().setTranslation(Mn::Vector3(state.position)); - const Eigen::Map rot(state.rotation.data()); - CORRADE_ASSERT(std::abs(rot.norm() - 1.0) < - 2.0 * Magnum::Math::TypeTraits::epsilon(), + const Mn::Quaternion rot = + Mn::Quaternion(state.rotation.xyz(), state.rotation.w()); + CORRADE_ASSERT(rot.isNormalized(), state.rotation << " not a valid rotation", ); - node().setRotation(Magnum::Quaternion(quatf(rot)).normalized()); + node().setRotation(rot); if (resetSensors) { for (auto& p : node().getNodeSensors()) { diff --git a/src/esp/agent/Agent.h b/src/esp/agent/Agent.h index ca94bd442a..d2a0e2ef8e 100644 --- a/src/esp/agent/Agent.h +++ b/src/esp/agent/Agent.h @@ -10,7 +10,6 @@ #include #include "esp/core/Esp.h" -#include "esp/core/EspEigen.h" #include "esp/scene/SceneNode.h" namespace esp { @@ -31,13 +30,13 @@ struct AgentState { /** * @brief the position of the agent */ - vec3f position = {0, 0, 0}; + Magnum::Vector3 position{0, 0, 0}; /** * @brief the agent's rotation. TODO : This exposes the rotation quaternion - * x,y,z,w as vec4f for pybind11 interop, replace with quatf when we have - * custom pybind11 type conversion for quaternions + * x,y,z,w as Magnum Vector4 for pybind11 interop, replace with quatf when we + * have custom pybind11 type conversion for quaternions */ - vec4f rotation = {0, 0, 0, 1}; + Magnum::Vector4 rotation = {0, 0, 0, 1}; ESP_SMART_POINTERS(AgentState) }; 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 f0ad6df3fd..fcf48e201f 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 @@ -686,11 +684,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) @@ -3486,8 +3481,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); @@ -3534,8 +3528,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 644d6f933c..c8f9105f81 100644 --- a/src/esp/bindings/SceneBindings.cpp +++ b/src/esp/bindings/SceneBindings.cpp @@ -254,11 +254,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 71574f11e9..cdb7a186df 100644 --- a/src/esp/bindings/ShortestPathBindings.cpp +++ b/src/esp/bindings/ShortestPathBindings.cpp @@ -175,24 +175,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/bindings_js/bindings_js.cpp b/src/esp/bindings_js/bindings_js.cpp index 85d38c9363..e1e436f270 100644 --- a/src/esp/bindings_js/bindings_js.cpp +++ b/src/esp/bindings_js/bindings_js.cpp @@ -77,8 +77,8 @@ static inline auto create(Targs&&... args) { return std::make_shared(std::forward(args)...); } -Magnum::Quaternion toQuaternion(const vec4f& rot) { - return Magnum::Quaternion(quatf(rot)).normalized(); +Magnum::Quaternion toQuaternion(const Magnum::Vector4& rot) { + return Magnum::Quaternion({rot[0], rot[1], rot[2]}, rot[3]).normalized(); } Magnum::Quaternion Quaternion_mul(const Magnum::Quaternion& q1, @@ -103,23 +103,15 @@ Observation Sensor_getObservation(Sensor& sensor, Simulator& sim) { return ret; } -vec3f toVec3f(const Magnum::Vector3& pos) { - return vec3f(pos.x(), pos.y(), pos.z()); -} - -vec4f toVec4f(const Magnum::Quaternion& rot) { - return vec4f(rot.vector().x(), rot.vector().y(), rot.vector().z(), - rot.scalar()); -} - void Sensor_setLocalTransform(Sensor& sensor, - const vec3f& pos, - const vec4f& rot) { + const Magnum::Vector3& pos, + const Magnum::Vector4& rot) { SceneNode& node{sensor.node()}; node.resetTransformation(); - node.translate(Magnum::Vector3(pos)); - node.setRotation(Magnum::Quaternion(quatf(rot)).normalized()); + node.translate(pos); + node.setRotation( + Magnum::Quaternion({rot[0], rot[1], rot[2]}, rot[3]).normalized()); } /** @@ -144,8 +136,6 @@ EMSCRIPTEN_BINDINGS(habitat_sim_bindings_js) { em::constant("_loggingContext", std::make_shared()); em::function("toQuaternion", &toQuaternion); - em::function("toVec3f", &toVec3f); - em::function("toVec4f", &toVec4f); em::function("loadAllObjectConfigsFromPath", &loadAllObjectConfigsFromPath); em::function("isBuildWithBulletPhysics", &isBuildWithBulletPhysics); @@ -164,40 +154,6 @@ EMSCRIPTEN_BINDINGS(habitat_sim_bindings_js) { em::register_map("MapStringObservation"); em::register_map("ActionSpace"); - em::value_array("vec2f") - .element(em::index<0>()) - .element(em::index<1>()); - - em::value_array("vec3f") - .element(em::index<0>()) - .element(em::index<1>()) - .element(em::index<2>()); - - em::value_array("vec4f") - .element(em::index<0>()) - .element(em::index<1>()) - .element(em::index<2>()) - .element(em::index<3>()); - - em::value_array("vec2i") - .element(em::index<0>()) - .element(em::index<1>()); - - em::value_array("vec3i") - .element(em::index<0>()) - .element(em::index<1>()) - .element(em::index<2>()); - - em::value_array("vec4i") - .element(em::index<0>()) - .element(em::index<1>()) - .element(em::index<2>()) - .element(em::index<3>()); - - em::value_object>("aabb") - .field("min", &std::pair::first) - .field("max", &std::pair::second); - em::class_("Rad").constructor(); em::class_("Vector3") diff --git a/src/esp/bindings_js/modules/topdown.js b/src/esp/bindings_js/modules/topdown.js index 8452d4f594..5a7f592a65 100644 --- a/src/esp/bindings_js/modules/topdown.js +++ b/src/esp/bindings_js/modules/topdown.js @@ -41,7 +41,7 @@ class TopDownMap { /** * Set the start position of the trajectory. - * @param {vec3f} position - start position + * @param {Magnum::Vector3} position - start position */ start(position) { this.currentY = position[1]; @@ -54,7 +54,7 @@ class TopDownMap { /** * Update trajectory with new position - * @param {vec3f} position - new position + * @param {Magnum::Vector3} position - new position * @param {int} throttleMs - time gap for throttle */ moveTo(position, throttleMs = 0) { diff --git a/src/esp/bindings_js/modules/vr_utils.js b/src/esp/bindings_js/modules/vr_utils.js index 62af3c83f9..5242b9399b 100644 --- a/src/esp/bindings_js/modules/vr_utils.js +++ b/src/esp/bindings_js/modules/vr_utils.js @@ -68,14 +68,10 @@ export function updateHeadPose(pose, agent) { const pos = pointToArray(view.transform.position).slice(0, -1); // don't need w for position sensor.setLocalTransform( - Module.toVec3f( - inverseAgentRot.transformVector(new Module.Vector3(...pos)) - ), - Module.toVec4f( - Module.Quaternion.mul( - inverseAgentRot, - Module.toQuaternion(pointToArray(view.transform.orientation)) - ) + inverseAgentRot.transformVector(new Module.Vector3(...pos)), + Module.Quaternion.mul( + inverseAgentRot, + Module.toQuaternion(pointToArray(view.transform.orientation)) ) ); } 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 d8023c9fc7..a18d6df2e5 100644 --- a/src/esp/core/Utility.h +++ b/src/esp/core/Utility.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include #include @@ -36,6 +38,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..f033bdff33 100644 --- a/src/esp/gfx/CubeMapCamera.cpp +++ b/src/esp/gfx/CubeMapCamera.cpp @@ -17,17 +17,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 d82547e3c0..319a948ed9 100644 --- a/src/esp/gfx/RenderCamera.cpp +++ b/src/esp/gfx/RenderCamera.cpp @@ -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 9d94703876..aeeba578b0 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 cf630f5cc6..6bec84e526 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..158c9a157a 100644 --- a/src/esp/nav/GreedyFollower.cpp +++ b/src/esp/nav/GreedyFollower.cpp @@ -39,8 +39,8 @@ GreedyGeodesicFollowerImpl::GreedyGeodesicFollowerImpl( 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 +56,7 @@ GreedyGeodesicFollowerImpl::TryStepResult GreedyGeodesicFollowerImpl::tryStep( const float geoDistAfter = geoDist(newPose, end); const float distToObsAfter = pathfinder_->distanceToClosestObstacle( - cast(newPose), 1.1 * closeToObsThreshold_); + newPose, 1.1 * closeToObsThreshold_); return {geoDistAfter, distToObsAfter, didCollide}; } @@ -163,8 +163,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 +200,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/PathFinder.cpp b/src/esp/nav/PathFinder.cpp index 1e7753e9fc..2354a0a11d 100644 --- a/src/esp/nav/PathFinder.cpp +++ b/src/esp/nav/PathFinder.cpp @@ -3,12 +3,14 @@ // LICENSE file in the root directory of this source tree. #include "PathFinder.h" +#include #include #include #include #include #include +#include #include #include @@ -18,6 +20,7 @@ #include #include + // NOLINTNEXTLINE #define _USE_MATH_DEFINES #include @@ -26,6 +29,7 @@ #include "esp/assets/MeshData.h" #include "esp/core/Esp.h" +#include "esp/core/EspEigen.h" #include "DetourCommon.h" #include "DetourNavMesh.h" @@ -93,23 +97,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()} {}; void MultiGoalShortestPath::setRequestedEnds( - const std::vector& newEnds) { + const std::vector& newEnds) { pimpl_->endRefs.clear(); pimpl_->pathEnds.clear(); pimpl_->requestedEnds = newEnds; @@ -117,13 +121,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 +139,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 +171,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 +193,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 +201,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 +332,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 +414,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 +434,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 +477,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,18 +516,18 @@ 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 distanceToClosestObstacle(const Mn::Vector3& pt, float maxSearchRadius = 2.0) const; - HitRecord closestObstacleSurfacePoint(const vec3f& pt, + HitRecord closestObstacleSurfacePoint(const Mn::Vector3& pt, float maxSearchRadius = 2.0) const; - bool isNavigable(const vec3f& pt, float maxYDelta = 0.5) const; + bool isNavigable(const Mn::Vector3& pt, float maxYDelta = 0.5) const; - std::pair bounds() const { return bounds_; }; + std::pair bounds() const { return bounds_; }; Eigen::Matrix getTopDownView(float metersPerPixel, float height, float eps) const; @@ -552,21 +559,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 +925,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 +958,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 +995,7 @@ struct NavMeshTileHeader { }; struct Triangle { - std::vector v; + std::vector v; Triangle() { v.resize(3); } }; @@ -1006,13 +1013,13 @@ std::vector getPolygonTriangles(const dtPoly* poly, 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 +1033,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.5 * Mn::Math::cross(w1, w2).length(); } return area; @@ -1114,7 +1121,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 +1162,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,7 +1237,7 @@ 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); @@ -1249,7 +1256,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,13 +1279,13 @@ 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) { @@ -1297,7 +1304,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,13 +1330,13 @@ 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) { @@ -1349,11 +1356,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 +1372,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 +1390,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 +1427,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 +1456,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 +1473,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 +1493,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 +1518,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; @@ -1528,7 +1536,7 @@ bool PathFinder::Impl::findPath(MultiGoalShortestPath& path) { for (int 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 +1558,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 +1582,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 +1596,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 +1635,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 +1667,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 +1682,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 +1690,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 +1705,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 +1717,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,8 +1754,8 @@ 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-2) return false; return true; @@ -1758,9 +1767,9 @@ 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); + 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 +1783,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; } @@ -1790,9 +1799,9 @@ 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 +1815,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 +1838,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 @@ -1889,13 +1898,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 +1921,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 +1929,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 +1938,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 +1969,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 +1981,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,7 +2001,7 @@ 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(); } diff --git a/src/esp/nav/PathFinder.h b/src/esp/nav/PathFinder.h index bf782cb232..49c9cf4830 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 @@ -28,9 +29,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 +45,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 +58,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 +81,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 +97,7 @@ struct MultiGoalShortestPath { * * Will be empty if no path exists */ - std::vector points; + std::vector points; /** * @brief The geodesic distance @@ -386,8 +387,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 +407,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 +545,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 +572,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 +595,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 +610,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 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/ObjectControls.cpp b/src/esp/scene/ObjectControls.cpp index 1b9e45cf14..6108fd4628 100644 --- a/src/esp/scene/ObjectControls.cpp +++ b/src/esp/scene/ObjectControls.cpp @@ -4,18 +4,15 @@ #include "ObjectControls.h" -#include - #include #include "SceneNode.h" #include "esp/core/Esp.h" -using Magnum::EigenIntegration::cast; - namespace esp { namespace scene { +namespace Mn = Magnum; SceneNode& moveRight(SceneNode& object, float distance) { // TODO: this assumes no scale is applied object.translateLocal(object.transformation().right() * distance); @@ -95,13 +92,12 @@ ObjectControls& ObjectControls::action(SceneNode& object, if (moveFuncMapIter != moveFuncMap_.end()) { if (applyFilter) { // TODO: use magnum math for the filter func as well? - const auto startPosition = - cast(object.absoluteTransformation().translation()); + const auto startPosition = object.absoluteTransformation().translation(); moveFuncMapIter->second(object, distance); - const auto endPos = - cast(object.absoluteTransformation().translation()); - const vec3f filteredEndPosition = moveFilterFunc_(startPosition, endPos); - object.translate(Magnum::Vector3(vec3f(filteredEndPosition - endPos))); + const auto endPos = object.absoluteTransformation().translation(); + const Mn::Vector3 filteredEndPosition = + moveFilterFunc_(startPosition, endPos); + object.translate(filteredEndPosition - endPos); } else { moveFuncMapIter->second(object, distance); } diff --git a/src/esp/scene/ObjectControls.h b/src/esp/scene/ObjectControls.h index eb288607e8..540376b02a 100644 --- a/src/esp/scene/ObjectControls.h +++ b/src/esp/scene/ObjectControls.h @@ -5,12 +5,12 @@ #ifndef ESP_SCENE_OBJECTCONTROLS_H_ #define ESP_SCENE_OBJECTCONTROLS_H_ +#include #include #include #include #include "esp/core/Esp.h" -#include "esp/core/EspEigen.h" namespace esp { namespace scene { @@ -23,7 +23,9 @@ class ObjectControls { ObjectControls(); typedef std::function MoveFunc; - typedef std::function MoveFilterFunc; + typedef std::function + MoveFilterFunc; ObjectControls& setMoveFilterFunction(MoveFilterFunc filterFunc); ObjectControls& action(SceneNode& object, @@ -42,8 +44,10 @@ class ObjectControls { } protected: - MoveFilterFunc moveFilterFunc_ = [](const vec3f& /*start*/, - const vec3f& end) { return end; }; + MoveFilterFunc moveFilterFunc_ = [](const Magnum::Vector3& /*start*/, + const Magnum::Vector3& end) { + return end; + }; std::map moveFuncMap_; ESP_SMART_POINTERS(ObjectControls) 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..07d3f7715e 100644 --- a/src/esp/scene/SemanticScene.cpp +++ b/src/esp/scene/SemanticScene.cpp @@ -23,8 +23,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 +234,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 +260,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 +294,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 +591,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 bf4c230fba..6c9edc7295 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 ced37c3d7b..a4182d65a2 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 62cd613cd1..e143346dd4 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" @@ -875,22 +875,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()) { @@ -914,9 +912,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); } @@ -938,7 +935,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); } } } @@ -1001,9 +999,10 @@ bool Simulator::isNavMeshVisualizationActive() { void Simulator::sampleRandomAgentState(agent::AgentState& agentState) { if (pathfinder_->isLoaded()) { agentState.position = pathfinder_->getRandomNavigablePoint(); - const float randomAngleRad = random_->uniform_float_01() * M_PI; - quatf rotation(Eigen::AngleAxisf(randomAngleRad, vec3f::UnitY())); - agentState.rotation = rotation.coeffs(); + const Mn::Rad randomAngleRad{ + static_cast(random_->uniform_float_01() * M_PI)}; + auto rot = Mn::Quaternion::rotation(randomAngleRad, Mn::Vector3::yAxis()); + agentState.rotation = Mn::Vector4(rot.vector(), rot.scalar()); // TODO: any other AgentState members should be randomized? } else { ESP_ERROR() << "No loaded PathFinder, aborting sampleRandomAgentState."; @@ -1067,11 +1066,13 @@ agent::Agent::ptr Simulator::addAgent( if (pathfinder_->isLoaded()) { scene::ObjectControls::MoveFilterFunc moveFilterFunction; if (config_.allowSliding) { - moveFilterFunction = [&](const vec3f& start, const vec3f& end) { + moveFilterFunction = [&](const Mn::Vector3& start, + const Mn::Vector3& end) { return pathfinder_->tryStep(start, end); }; } else { - moveFilterFunction = [&](const vec3f& start, const vec3f& end) { + moveFilterFunction = [&](const Mn::Vector3& start, + const Mn::Vector3& end) { return pathfinder_->tryStepNoSliding(start, end); }; } diff --git a/src/tests/GeoTest.cpp b/src/tests/GeoTest.cpp index c3577d2f09..f585b44d34 100644 --- a/src/tests/GeoTest.cpp +++ b/src/tests/GeoTest.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "esp/core/Utility.h" #include "esp/geo/CoordinateFrame.h" #include "esp/geo/Geo.h" @@ -124,80 +125,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 aab59ccb37..75ccf16a98 100644 --- a/src/tests/NavTest.cpp +++ b/src/tests/NavTest.cpp @@ -19,7 +19,7 @@ namespace Cr = Corrade; namespace { -void printPathPoint(int run, int step, const esp::vec3f& p, float distance) { +void printPathPoint(int run, int step, const Mn::Vector3& p, float distance) { ESP_VERY_VERBOSE() << run << "," << step << "," << Mn::Vector3{p} << "," << distance; } @@ -60,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); @@ -80,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); @@ -95,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; } } } @@ -106,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, @@ -115,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); } @@ -131,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() { @@ -146,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..774b1968b6 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)); } } } @@ -150,17 +147,18 @@ void ReplicaSceneTest::testSemanticSceneLoading() { // 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/SimTest.cpp b/src/tests/SimTest.cpp index 50974ff674..71f12ddf2a 100644 --- a/src/tests/SimTest.cpp +++ b/src/tests/SimTest.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -64,6 +63,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(); @@ -89,8 +94,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"); @@ -119,8 +123,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"); @@ -472,7 +475,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 || @@ -511,7 +514,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)); @@ -655,8 +658,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); @@ -705,8 +707,7 @@ void SimTest::addObjectInvertedScale() { agent->setInitialState(AgentState{}); // 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, @@ -816,7 +817,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/datatool/Datatool.cpp b/src/utils/datatool/Datatool.cpp index e6310f1c97..2500ec6caf 100644 --- a/src/utils/datatool/Datatool.cpp +++ b/src/utils/datatool/Datatool.cpp @@ -13,6 +13,7 @@ #include "Mp3dInstanceMeshData.h" #include "esp/core/Esp.h" +#include "esp/core/Utility.h" #include "esp/nav/PathFinder.h" #include "esp/scene/SemanticScene.h" @@ -23,6 +24,7 @@ using esp::assets::SceneLoader; using esp::nav::NavMeshSettings; using esp::nav::PathFinder; using esp::scene::SemanticScene; +using Magnum::Math::Literals::operator""_degf; int createNavMesh(const std::string& meshFile, const std::string& navmeshFile) { SceneLoader loader; @@ -95,13 +97,13 @@ int createGibsonSemanticMesh(const std::string& objFile, f << "end_header" << std::endl; // We need to rotate to match .glb where -Z is gravity - const auto transform = - esp::quatf::FromTwoVectors(esp::vec3f::UnitY(), esp::vec3f::UnitZ()); + const Mn::Quaternion transform = + Mn::Quaternion::rotation(90.0_degf, Mn::Vector3::xAxis()); for (size_t i = 0; i < numVerts; ++i) { unsigned char gray[] = {0x80, 0x80, 0x80}; float* components = &attrib.vertices[i * 3]; - Eigen::Map vertex{components}; - vertex = transform * vertex; + Mn::Vector3 vertex = Mn::Vector3::from(components); + vertex = transform.transformVectorNormalized(vertex); f.write(reinterpret_cast(components), sizeof(float) * 3); f.write(reinterpret_cast(gray), sizeof(gray)); } diff --git a/src/utils/datatool/Mp3dInstanceMeshData.cpp b/src/utils/datatool/Mp3dInstanceMeshData.cpp index f3f7880891..d47584cdfd 100644 --- a/src/utils/datatool/Mp3dInstanceMeshData.cpp +++ b/src/utils/datatool/Mp3dInstanceMeshData.cpp @@ -25,6 +25,8 @@ namespace esp { namespace assets { +namespace Mn = Magnum; + bool Mp3dInstanceMeshData::loadMp3dPLY(const std::string& plyFile) { std::ifstream ifs(plyFile); if (!ifs.good()) { @@ -84,10 +86,10 @@ bool Mp3dInstanceMeshData::loadMp3dPLY(const std::string& plyFile) { perFaceIdxs_.reserve(nFace); for (int i = 0; i < nVertex; ++i) { - vec3f position; - vec3f normal; - vec2f texCoords; - vec3uc rgb; + Mn::Vector3 position; + Mn::Vector3 normal; + Mn::Vector2 texCoords; + Mn::Vector3ub rgb; ifs.read(reinterpret_cast(position.data()), 3 * sizeof(float)); ifs.read(reinterpret_cast(normal.data()), 3 * sizeof(float)); @@ -99,7 +101,7 @@ bool Mp3dInstanceMeshData::loadMp3dPLY(const std::string& plyFile) { for (int i = 0; i < nFace; ++i) { uint8_t nIndices = 0; - vec3ui indices; + Mn::Vector3ui indices; int32_t materialId = 0; int32_t segmentId = 0; int32_t categoryId = 0; @@ -141,15 +143,15 @@ bool Mp3dInstanceMeshData::saveSemMeshPLY( f << "end_header" << std::endl; for (int iVertex = 0; iVertex < nVertex; ++iVertex) { - const vec3f& xyz = cpu_vbo_[iVertex].head<3>(); - const vec3uc& rgb = cpu_cbo_[iVertex]; + const Mn::Vector3& xyz = cpu_vbo_[iVertex]; + const Mn::Vector3ub& rgb = cpu_cbo_[iVertex]; f.write(reinterpret_cast(xyz.data()), 3 * sizeof(float)); f.write(reinterpret_cast(rgb.data()), 3 * sizeof(uint8_t)); } for (int iFace = 0; iFace < perFaceIdxs_.size(); ++iFace) { const uint8_t nIndices = 3; - const vec3ui& indices = perFaceIdxs_[iFace]; + const Mn::Vector3ui& indices = perFaceIdxs_[iFace]; // The materialId corresponds to the segmentId from the .house file const int32_t segmentId = materialIds_[iFace]; int32_t objectId = ID_UNDEFINED; diff --git a/src/utils/datatool/Mp3dInstanceMeshData.h b/src/utils/datatool/Mp3dInstanceMeshData.h index 6ae9f6b26c..4b1d7d65e2 100644 --- a/src/utils/datatool/Mp3dInstanceMeshData.h +++ b/src/utils/datatool/Mp3dInstanceMeshData.h @@ -13,7 +13,6 @@ #include #include "esp/core/Esp.h" -#include "esp/core/EspEigen.h" namespace esp { namespace assets { @@ -36,9 +35,9 @@ class Mp3dInstanceMeshData { const std::unordered_map& segmentIdToObjectIdMap); protected: - std::vector cpu_vbo_; - std::vector cpu_cbo_; - std::vector perFaceIdxs_; + std::vector cpu_vbo_; + std::vector cpu_cbo_; + std::vector perFaceIdxs_; std::vector materialIds_; std::vector segmentIds_; std::vector categoryIds_; diff --git a/src/utils/datatool/SceneLoader.cpp b/src/utils/datatool/SceneLoader.cpp index 4e9c95a349..841e843237 100644 --- a/src/utils/datatool/SceneLoader.cpp +++ b/src/utils/datatool/SceneLoader.cpp @@ -14,6 +14,7 @@ #include #include "esp/assets/GenericSemanticMeshData.h" #include "esp/core/Esp.h" +#include "esp/core/Utility.h" #include "esp/geo/Geo.h" #include "esp/metadata/attributes/AttributesEnumMaps.h" @@ -21,8 +22,6 @@ #include #include -#include -#include #include namespace Cr = Corrade; @@ -71,8 +70,7 @@ MeshData SceneLoader::load(const AssetInfo& info) { Cr::Containers::arrayView(mesh.vbo))); mesh.ibo = ibo; for (const auto& c : cbo) { - auto clr = Mn::EigenIntegration::cast(c); - mesh.cbo.emplace_back(clr.cast() / 255.0f); + mesh.cbo.emplace_back(c / 255.0f); } } else { const aiScene* scene; @@ -84,32 +82,36 @@ MeshData SceneLoader::load(const AssetInfo& info) { scene = Importer.ReadFile(info.filepath.c_str(), assimpFlags); - const quatf alignSceneToEspGravity = - quatf::FromTwoVectors(info.frame.gravity(), esp::geo::ESP_GRAVITY); + const Mn::Quaternion alignSceneToEspGravity = + Mn::Quaternion::rotation(info.frame.gravity(), esp::geo::ESP_GRAVITY); // Iterate through all meshes in the file and extract the vertex components for (uint32_t m = 0, indexBase = 0; m < scene->mNumMeshes; ++m) { const aiMesh& assimpMesh = *scene->mMeshes[m]; for (uint32_t v = 0; v < assimpMesh.mNumVertices; ++v) { - // Use Eigen::Map to convert ASSIMP vectors to eigen vectors - const Eigen::Map xyz_scene(&assimpMesh.mVertices[v].x); - const vec3f xyz_esp = alignSceneToEspGravity * xyz_scene; + const Mn::Vector3 xyz_scene = + Mn::Vector3::from(&assimpMesh.mVertices[v].x); + const Mn::Vector3 xyz_esp = + alignSceneToEspGravity.transformVectorNormalized(xyz_scene); mesh.vbo.push_back(xyz_esp); if (assimpMesh.mNormals) { - const Eigen::Map normal_scene(&assimpMesh.mNormals[v].x); - const vec3f normal_esp = alignSceneToEspGravity * normal_scene; + const Mn::Vector3 normal_scene = + Mn::Vector3::from(&assimpMesh.mNormals[v].x); + const Mn::Vector3 normal_esp = + alignSceneToEspGravity.transformVectorNormalized(normal_scene); mesh.nbo.push_back(normal_esp); } if (assimpMesh.HasTextureCoords(0)) { - const Eigen::Map texCoord( - &assimpMesh.mTextureCoords[0][v].x); + const Mn::Vector2 texCoord = + Mn::Vector2::from(&assimpMesh.mTextureCoords[0][v].x); mesh.tbo.push_back(texCoord); } if (assimpMesh.HasVertexColors(0)) { - const Eigen::Map color(&assimpMesh.mColors[0][v].r); + const Mn::Vector3 color = + Mn::Vector3::from(&assimpMesh.mColors[0][v].r); mesh.cbo.push_back(color); } } // vertices diff --git a/src/utils/viewer/viewer.cpp b/src/utils/viewer/viewer.cpp index b66549e933..faaa842c26 100644 --- a/src/utils/viewer/viewer.cpp +++ b/src/utils/viewer/viewer.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -419,20 +418,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; } } @@ -1174,7 +1174,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); @@ -1223,7 +1223,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!! @@ -2273,7 +2273,7 @@ void Viewer::keyPressEvent(KeyEvent& event) { break; case KeyEvent::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/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/tests/test_random_seed.py b/tests/test_random_seed.py index c77849bc78..36e9542ed2 100644 --- a/tests/test_random_seed.py +++ b/tests/test_random_seed.py @@ -19,17 +19,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