Skip to content

Commit

Permalink
--reviewer suggestions
Browse files Browse the repository at this point in the history
  • Loading branch information
jturner65 committed Jan 25, 2024
1 parent 3f6882e commit 4aa0dff
Show file tree
Hide file tree
Showing 11 changed files with 72 additions and 76 deletions.
32 changes: 15 additions & 17 deletions src/esp/geo/CoordinateFrame.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,49 +11,47 @@
namespace esp {
namespace geo {

namespace Mn = Magnum;

//! Represents a frame of reference defined by an origin and
//! two semantically meaningful directions: "up" and "front", or
//! equivalently "gravity" and "back"
class CoordinateFrame {
public:
explicit CoordinateFrame(const Mn::Vector3& up = ESP_UP,
const Mn::Vector3& front = ESP_FRONT,
const Mn::Vector3& origin = {});
explicit CoordinateFrame(const Mn::Quaternion& rotation,
const Mn::Vector3& origin = {});
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
Mn::Vector3 origin() const { return origin_; }
Magnum::Vector3 origin() const { return origin_; }

//! Returns up orientation
Mn::Vector3 up() const { return up_; }
Magnum::Vector3 up() const { return up_; }

//! Returns down/gravity orientation
Mn::Vector3 gravity() const { return -up_; }
Magnum::Vector3 gravity() const { return -up_; }

//! Returns front orientation
Mn::Vector3 front() const { return front_; }
Magnum::Vector3 front() const { return front_; }

//! Returns front orientation
Mn::Vector3 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
Mn::Quaternion rotationWorldToFrame() const;
Magnum::Quaternion rotationWorldToFrame() const;

//! Returns quaternion representing the rotation taking direction vectors in
//! this CoordinateFrame to direction vectors in world coordinates
Mn::Quaternion rotationFrameToWorld() const;
Magnum::Quaternion rotationFrameToWorld() const;

//! Returns a stringified JSON representation of this CoordinateFrame
std::string toString() const;

protected:
Mn::Vector3 up_;
Mn::Vector3 front_;
Mn::Vector3 origin_;
Magnum::Vector3 up_;
Magnum::Vector3 front_;
Magnum::Vector3 origin_;
ESP_SMART_POINTERS(CoordinateFrame)
};

Expand Down
3 changes: 2 additions & 1 deletion src/esp/nav/PathFinder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1372,7 +1372,8 @@ Mn::Vector3 PathFinder::Impl::getRandomNavigablePointAroundSphere(
status = navQuery_->findRandomPointAroundCircle(
start_ref, circleCenter.data(), radius, filter_.get(), frand,
&rand_ref, pt.data());
if (dtStatusSucceed(status) && (pt - circleCenter).length() <= radius) {
if (dtStatusSucceed(status) &&
(pt - circleCenter).dot() <= radius * radius) {
break;
}
}
Expand Down
3 changes: 1 addition & 2 deletions src/esp/scene/ObjectControls.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,8 +97,7 @@ ObjectControls& ObjectControls::action(SceneNode& object,
const auto endPos = object.absoluteTransformation().translation();
const Mn::Vector3 filteredEndPosition =
moveFilterFunc_(startPosition, endPos);
object.translate(
Magnum::Vector3(Mn::Vector3(filteredEndPosition - endPos)));
object.translate(filteredEndPosition - endPos);
} else {
moveFuncMapIter->second(object, distance);
}
Expand Down
7 changes: 4 additions & 3 deletions src/esp/scene/ObjectControls.h
Original file line number Diff line number Diff line change
Expand Up @@ -44,9 +44,10 @@ class ObjectControls {
}

protected:
MoveFilterFunc moveFilterFunc_ =
[](CORRADE_UNUSED const Magnum::Vector3& start,
const Magnum::Vector3& end) { return end; };
MoveFilterFunc moveFilterFunc_ = [](const Magnum::Vector3& /*start*/,
const Magnum::Vector3& end) {
return end;
};
std::map<std::string, MoveFunc> moveFuncMap_;

ESP_SMART_POINTERS(ObjectControls)
Expand Down
8 changes: 3 additions & 5 deletions src/esp/scene/ReplicaSemanticScene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,9 @@ namespace scene {

constexpr int kMaxIds = 10000; /* We shouldn't every need more than this. */

bool SemanticScene::loadReplicaHouse(
const std::string& houseFilename,
SemanticScene& scene,
const Magnum::Quaternion&
worldRotation /* = Magnum::Quaternion::Identity() */) {
bool SemanticScene::loadReplicaHouse(const std::string& houseFilename,
SemanticScene& scene,
const Magnum::Quaternion& worldRotation) {
if (!checkFileExists(houseFilename, "loadReplicaHouse")) {
return false;
}
Expand Down
4 changes: 2 additions & 2 deletions src/esp/sensor/CameraSensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,8 @@ namespace {
hFoV from the CameraSensorSpec */
Mn::Matrix4 projectionMatrixInternal(const CameraSensorSpec& spec,
Mn::Rad hfov) {
const Mn::Vector2 nearPlaneSize{
1.0f, Mn::Vector2{Mn::Vector2i{spec.resolution}}.aspectRatio()};
const Mn::Vector2 nearPlaneSize{1.0f,
Mn::Vector2{spec.resolution}.aspectRatio()};
if (spec.sensorSubType == SensorSubType::Orthographic) {
return Mn::Matrix4::orthographicProjection(nearPlaneSize / spec.orthoScale,
spec.near, spec.far);
Expand Down
2 changes: 1 addition & 1 deletion src/esp/sensor/FisheyeSensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ Magnum::Vector2 computePrincipalPointOffset(const FisheyeSensorSpec& spec) {
if (bool(spec.principalPointOffset)) {
return *spec.principalPointOffset;
}
return Mn::Vector2(spec.resolution[0], spec.resolution[1]) * 0.5f;
return Mn::Vector2{spec.resolution} * 0.5f;
}

FisheyeSensor::FisheyeSensor(scene::SceneNode& cameraNode,
Expand Down
48 changes: 24 additions & 24 deletions src/tests/GeoTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -131,15 +131,15 @@ void GeoTest::obbConstruction() {
core::quatRotFromTwoVectors(Mn::Vector3::yAxis(), Mn::Vector3::zAxis());
OBB obb2(center, dimensions, rot1);

CORRADE_VERIFY(obb2.center() == center);
CORRADE_VERIFY(obb2.sizes() == dimensions);
CORRADE_VERIFY(obb2.halfExtents() == dimensions / 2);
CORRADE_VERIFY(obb2.rotation() == rot1);
CORRADE_COMPARE(obb2.center(), center);
CORRADE_COMPARE(obb2.sizes(), dimensions);
CORRADE_COMPARE(obb2.halfExtents(), dimensions / 2);
CORRADE_COMPARE(obb2.rotation(), rot1);

Mn::Range3D aabb(Mn::Vector3(-1, -2, -3), Mn::Vector3(3, 2, 1));
OBB obb3(aabb);
CORRADE_VERIFY(obb3.center() == aabb.center());
CORRADE_VERIFY(obb3.toAABB() == aabb);
CORRADE_COMPARE(obb3.center(), aabb.center());
CORRADE_COMPARE(obb3.toAABB(), aabb);
}

void GeoTest::obbFunctions() {
Expand All @@ -160,7 +160,7 @@ void GeoTest::obbFunctions() {
CORRADE_COMPARE(aabb.max(), (Mn::Vector3{10.0f, 5.0f, 1.0f}));

const auto identity = obb2.worldToLocal() * obb2.localToWorld();
CORRADE_VERIFY(identity == Mn::Matrix4());
CORRADE_COMPARE(identity, Mn::Matrix4());
CORRADE_VERIFY(
obb2.contains(obb2.localToWorld().transformPoint(Mn::Vector3(0, 0, 0))));
CORRADE_VERIFY(
Expand All @@ -182,25 +182,25 @@ void GeoTest::coordinateFrame() {
Mn::Matrix4 xform = Mn::Matrix4::from(rotation.toMatrix(), origin);

CoordinateFrame c1(up, front, origin);
CORRADE_VERIFY(c1.up() == up);
CORRADE_VERIFY(c1.gravity() == -up);
CORRADE_VERIFY(c1.front() == front);
CORRADE_VERIFY(c1.back() == -front);
CORRADE_VERIFY(c1.up() == rotation.transformVectorNormalized(ESP_UP));
CORRADE_VERIFY(c1.front() == rotation.transformVectorNormalized(ESP_FRONT));
CORRADE_VERIFY(c1.origin() == origin);
CORRADE_VERIFY(c1.rotationWorldToFrame() == 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() == up);
CORRADE_VERIFY(c2.gravity() == -up);
CORRADE_VERIFY(c2.front() == front);
CORRADE_VERIFY(c2.back() == -front);
CORRADE_VERIFY(c2.up() == rotation.transformVectorNormalized(ESP_UP));
CORRADE_VERIFY(c2.front() == rotation.transformVectorNormalized(ESP_FRONT));
CORRADE_VERIFY(c2.origin() == origin);
CORRADE_VERIFY(c2.rotationWorldToFrame() == rotation);
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);
Expand Down
4 changes: 1 addition & 3 deletions src/tests/ReplicaSceneTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@
#include <Corrade/TestSuite/Tester.h>

#include <Corrade/Utility/Path.h>
#include <Magnum/EigenIntegration/GeometryIntegration.h>
#include <Magnum/EigenIntegration/Integration.h>
#include <Magnum/Magnum.h>
#include <Magnum/Math/Vector3.h>

Expand Down Expand Up @@ -160,7 +158,7 @@ void ReplicaSceneTest::testSemanticSceneLoading() {
CORRADE_VERIFY(obj12->category());
CORRADE_COMPARE(obj12->category()->index(), 13);
CORRADE_COMPARE(obj12->category()->name(), "book");
} // namespace
} // ReplicaSceneTest::testSemanticSceneLoading

void ReplicaSceneTest::testSemanticSceneDescriptorReplicaCAD() {
if (!Cr::Utility::Path::exists(replicaCAD)) {
Expand Down
29 changes: 15 additions & 14 deletions src/tests/SimTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
#include <Corrade/Utility/Algorithms.h>
#include <Corrade/Utility/Path.h>
#include <Magnum/DebugTools/CompareImage.h>
#include <Magnum/EigenIntegration/Integration.h>
#include <Magnum/ImageView.h>
#include <Magnum/Magnum.h>
#include <Magnum/PixelFormat.h>
Expand Down Expand Up @@ -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();
Expand All @@ -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");
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -754,8 +757,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);
Expand Down Expand Up @@ -804,8 +806,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,
Expand Down Expand Up @@ -1102,15 +1103,15 @@ void SimTest::testArticulatedObjectSkinned() {
const auto linkIds = ao->getLinkIdsWithBase();

auto linkA = ao->getLink(linkIds[0]);
CORRADE_VERIFY(linkA->linkName == "A");
CORRADE_COMPARE(linkA->linkName, "A");
auto linkB = ao->getLink(linkIds[1]);
CORRADE_VERIFY(linkB->linkName == "B");
CORRADE_COMPARE(linkB->linkName, "B");
auto linkC = ao->getLink(linkIds[2]);
CORRADE_VERIFY(linkC->linkName == "C");
CORRADE_COMPARE(linkC->linkName, "C");
auto linkD = ao->getLink(linkIds[3]);
CORRADE_VERIFY(linkD->linkName == "D");
CORRADE_COMPARE(linkD->linkName, "D");
auto linkE = ao->getLink(linkIds[4]);
CORRADE_VERIFY(linkE->linkName == "E");
CORRADE_COMPARE(linkE->linkName, "E");

ao->setTranslation({1.f, -3.f, -6.f});

Expand Down
8 changes: 4 additions & 4 deletions src/utils/viewer/viewer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -687,7 +687,7 @@ void addSensors(esp::agent::AgentConfiguration& agentConfig, bool isOrtho) {
}
spec->position = {0.0f, rgbSensorHeight, 0.0f};
spec->orientation = {0, 0, 0};
spec->resolution = Mn::Vector2i(viewportSize[1], viewportSize[0]);
spec->resolution = viewportSize.flipped();
};
// add the camera color sensor
// for historical reasons, we call it "rgba_camera"
Expand Down Expand Up @@ -716,7 +716,7 @@ void addSensors(esp::agent::AgentConfiguration& agentConfig, bool isOrtho) {
}
spec->sensorSubType = esp::sensor::SensorSubType::Fisheye;
spec->fisheyeModelType = modelType;
spec->resolution = Mn::Vector2i(viewportSize[1], viewportSize[0]);
spec->resolution = viewportSize.flipped();
// default viewport size: 1600 x 1200
spec->principalPointOffset =
Mn::Vector2(viewportSize[0] / 2, viewportSize[1] / 2);
Expand Down Expand Up @@ -761,7 +761,7 @@ void addSensors(esp::agent::AgentConfiguration& agentConfig, bool isOrtho) {
spec->channels = 1;
}
spec->sensorSubType = esp::sensor::SensorSubType::Equirectangular;
spec->resolution = Mn::Vector2i(viewportSize[1], viewportSize[0]);
spec->resolution = viewportSize.flipped();
};
// add the equirectangular sensor
addEquirectangularSensor("rgba_equirectangular",
Expand Down Expand Up @@ -2258,7 +2258,7 @@ void Viewer::keyPressEvent(KeyEvent& event) {
break;
case KeyEvent::Key::Nine:
if (simulator_->getPathFinder()->isLoaded()) {
const auto position =
const Mn::Vector3 position =
simulator_->getPathFinder()->getRandomNavigablePoint();
agentBodyNode_->setTranslation(Mn::Vector3(position));
}
Expand Down

0 comments on commit 4aa0dff

Please sign in to comment.