diff --git a/tools/code_check.sh b/tools/code_check.sh index 496b0ce41..f583bedcf 100755 --- a/tools/code_check.sh +++ b/tools/code_check.sh @@ -45,7 +45,7 @@ then CPPLINT_FILES="$CHECK_FILES" QUICK_TMP=`mktemp -t asdfXXXXXXXXXX` else - CHECK_DIRS="./src ./include ./test/integration ./test/regression ./test/performance" + CHECK_DIRS="./src ./include ./test/integration ./test/regression ./test/performance ./tpe" if [ $CPPCHECK_LT_157 -eq 1 ]; then # cppcheck is older than 1.57, so don't check header files (issue #907) CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc"` diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index 95c8a6014..d17963882 100644 --- a/tpe/lib/src/Collision.cc +++ b/tpe/lib/src/Collision.cc @@ -74,7 +74,8 @@ void Collision::SetShape(const Shape &_shape) } else if (_shape.GetType() == ShapeType::CYLINDER) { - const CylinderShape *typedShape = static_cast(&_shape); + const CylinderShape *typedShape = + static_cast(&_shape); this->dataPtr->shape.reset(new CylinderShape(*typedShape)); } else if (_shape.GetType() == ShapeType::SPHERE) diff --git a/tpe/lib/src/Collision.hh b/tpe/lib/src/Collision.hh index 9bccfe5ad..30ad500ea 100644 --- a/tpe/lib/src/Collision.hh +++ b/tpe/lib/src/Collision.hh @@ -38,7 +38,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Collision : public Entity /// \brief Constructor /// \param[in] _id Collision id - public: Collision(std::size_t _id); + public: explicit Collision(std::size_t _id); /// \brief Copy Constructor /// \param[in] _other The other collision to copy from diff --git a/tpe/lib/src/Engine.cc b/tpe/lib/src/Engine.cc index 69c3ff8cd..7e8abeb27 100644 --- a/tpe/lib/src/Engine.cc +++ b/tpe/lib/src/Engine.cc @@ -34,7 +34,8 @@ Engine::Engine() Entity &Engine::AddWorld() { auto world = std::make_shared(); - const auto [it, success] = this->worlds.insert({world->GetId(), world}); + const auto[it, success] = + this->worlds.insert({world->GetId(), world}); return *it->second; } diff --git a/tpe/lib/src/Engine.hh b/tpe/lib/src/Engine.hh index baec909c6..841292b16 100644 --- a/tpe/lib/src/Engine.hh +++ b/tpe/lib/src/Engine.hh @@ -18,6 +18,7 @@ #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_ENGINE_HH_ #define IGNITION_PHYSICS_TPE_LIB_SRC_ENGINE_HH_ +#include #include #include @@ -67,8 +68,8 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Engine IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING }; -} // namespace tpelib -} // namespace physics -} // namespace ignition +} // namespace tpelib +} // namespace physics +} // namespace ignition #endif diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc index cc99053a0..a8cb933f2 100644 --- a/tpe/lib/src/Link.cc +++ b/tpe/lib/src/Link.cc @@ -36,7 +36,7 @@ Link::Link(std::size_t _id) : Entity(_id) Entity &Link::AddCollision() { std::size_t collisionId = Entity::GetNextId(); - const auto [it, success] = this->GetChildren().insert( + const auto[it, success] = this->GetChildren().insert( {collisionId, std::make_shared(collisionId)}); return *it->second.get(); } diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh index 9730e77f8..6cf68f55b 100644 --- a/tpe/lib/src/Link.hh +++ b/tpe/lib/src/Link.hh @@ -34,7 +34,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity /// \brief Constructor /// \param[in] _id Link id - public: Link(std::size_t _id); + public: explicit Link(std::size_t _id); /// \brief Destructor public: ~Link() = default; diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index 768ac5281..37d335904 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -41,7 +41,7 @@ Model::Model(std::size_t _id) : Entity(_id) Entity &Model::AddLink() { std::size_t linkId = Entity::GetNextId(); - const auto [it, success] = this->GetChildren().insert( + const auto[it, success] = this->GetChildren().insert( {linkId, std::make_shared(linkId)}); return *it->second.get(); } diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh index 623b2f4bb..d53c75c13 100644 --- a/tpe/lib/src/Model.hh +++ b/tpe/lib/src/Model.hh @@ -38,7 +38,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity /// \brief Constructor /// \param[in] _id Model id - public: Model(std::size_t _id); + public: explicit Model(std::size_t _id); /// \brief Destructor public: ~Model() = default; diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc index 55b81ae5f..8ba0844f1 100644 --- a/tpe/lib/src/Model_TEST.cc +++ b/tpe/lib/src/Model_TEST.cc @@ -55,7 +55,8 @@ TEST(Model, BasicAPI) math::Pose3d expectedPose( originalPose.Pos() + math::Vector3d(0.1, 0.1, 0.1) * timeStep, originalPose.Rot().Integrate(math::Vector3d(1.0, 0, 0), timeStep)); - model2.UpdatePose(timeStep, model2.GetLinearVelocity(), model2.GetAngularVelocity()); + model2.UpdatePose( + timeStep, model2.GetLinearVelocity(), model2.GetAngularVelocity()); EXPECT_EQ(expectedPose, model2.GetPose()); } diff --git a/tpe/lib/src/Shape.cc b/tpe/lib/src/Shape.cc index 33c63d9af..6337aebd3 100644 --- a/tpe/lib/src/Shape.cc +++ b/tpe/lib/src/Shape.cc @@ -27,16 +27,16 @@ Shape::Shape() this->type = ShapeType::EMPTY; } - ////////////////////////////////////////////////// - math::AxisAlignedBox Shape::GetBoundingBox() - { - if (this->dirty) - { - this->UpdateBoundingBox(); - this->dirty = false; - } - return this->bbox; - } +////////////////////////////////////////////////// +math::AxisAlignedBox Shape::GetBoundingBox() +{ + if (this->dirty) + { + this->UpdateBoundingBox(); + this->dirty = false; + } + return this->bbox; +} ////////////////////////////////////////////////// void Shape::UpdateBoundingBox() diff --git a/tpe/lib/src/Shape.hh b/tpe/lib/src/Shape.hh index 82bfdb2eb..d1d7ddc59 100644 --- a/tpe/lib/src/Shape.hh +++ b/tpe/lib/src/Shape.hh @@ -76,13 +76,13 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Shape /// \brief Update the shape's bounding box protected: virtual void UpdateBoundingBox(); - /// \brief Bounding Box + /// \brief Bounding Box protected: math::AxisAlignedBox bbox; - /// \brief Type of shape + /// \brief Type of shape protected: ShapeType type; - /// \brief Flag to indicate if dimensions changed + /// \brief Flag to indicate if dimensions changed protected: bool dirty = true; }; diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc index 248e0a76d..d83b62d7a 100644 --- a/tpe/lib/src/World.cc +++ b/tpe/lib/src/World.cc @@ -80,7 +80,7 @@ void World::Step() Entity &World::AddModel() { std::size_t modelId = Entity::GetNextId(); - const auto [it, success] = this->GetChildren().insert( + const auto[it, success] = this->GetChildren().insert( {modelId, std::make_shared(modelId)}); return *it->second.get(); } diff --git a/tpe/lib/src/World.hh b/tpe/lib/src/World.hh index 10eb43efb..498e6bf39 100644 --- a/tpe/lib/src/World.hh +++ b/tpe/lib/src/World.hh @@ -65,9 +65,8 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE World : public Entity protected: double timeStep{0.1}; }; -} // namespace tpelib -} // namespace physics -} // namespace ignition - +} // namespace tpelib +} // namespace physics +} // namespace ignition #endif diff --git a/tpe/plugin/src/Base.hh b/tpe/plugin/src/Base.hh index 8c7796334..97a4170db 100644 --- a/tpe/plugin/src/Base.hh +++ b/tpe/plugin/src/Base.hh @@ -127,7 +127,7 @@ class Base : public Implements3d> modelPtr->model = &_model; size_t modelId = _model.GetId(); this->models.insert({modelId, modelPtr}); - // keep track of model's corresponding world + // keep track of model's corresponding world this->childIdToParentId.insert({modelId, _worldId}); return this->GenerateIdentity(modelId, modelPtr); diff --git a/tpe/plugin/src/Base_TEST.cc b/tpe/plugin/src/Base_TEST.cc index c4926e50f..551fe0241 100644 --- a/tpe/plugin/src/Base_TEST.cc +++ b/tpe/plugin/src/Base_TEST.cc @@ -15,10 +15,10 @@ * */ -#include - #include +#include + #include #include @@ -50,21 +50,23 @@ TEST(BaseClass, AddEntities) EXPECT_TRUE(base.worlds.find(worldId) != base.worlds.end()); EXPECT_EQ(worldId, base.worlds.find(worldId)->second->world->GetId()); - EXPECT_EQ(world->GetName(), base.worlds.find(worldId)->second->world->GetName()); + EXPECT_EQ( + world->GetName(), base.worlds.find(worldId)->second->world->GetName()); // add models to world auto &modelEnt1 = world->AddModel(); modelEnt1.SetName("box"); auto *model1 = static_cast(&modelEnt1); std::size_t modelId1 = model1->GetId(); - + EXPECT_EQ(0u, base.models.size()); auto modelIdentity1 = base.AddModel(worldId, *model1); EXPECT_EQ(1u, base.models.size()); EXPECT_TRUE(base.models.find(modelId1) != base.models.end()); EXPECT_EQ(modelId1, base.models.find(modelId1)->second->model->GetId()); - EXPECT_EQ(model1->GetName(), base.models.find(modelId1)->second->model->GetName()); + EXPECT_EQ( + model1->GetName(), base.models.find(modelId1)->second->model->GetName()); auto &modelEnt2 = world->AddModel(); modelEnt2.SetName("cylinder"); @@ -77,7 +79,8 @@ TEST(BaseClass, AddEntities) EXPECT_TRUE(base.models.find(modelId2) != base.models.end()); EXPECT_EQ(modelId2, base.models.find(modelId2)->second->model->GetId()); - EXPECT_EQ(model2->GetName(), base.models.find(modelId2)->second->model->GetName()); + EXPECT_EQ( + model2->GetName(), base.models.find(modelId2)->second->model->GetName()); // add first link to model1 auto &linkEnt1 = model1->AddLink(); @@ -92,7 +95,8 @@ TEST(BaseClass, AddEntities) EXPECT_TRUE(base.links.find(linkId1) != base.links.end()); EXPECT_EQ(linkId1, base.links.find(linkId1)->second->link->GetId()); - EXPECT_EQ(link1->GetName(), base.links.find(linkId1)->second->link->GetName()); + EXPECT_EQ( + link1->GetName(), base.links.find(linkId1)->second->link->GetName()); EXPECT_EQ(modelId1, base.childIdToParentId.find(linkId1)->second); // add second link to model2 @@ -108,7 +112,8 @@ TEST(BaseClass, AddEntities) EXPECT_TRUE(base.links.find(linkId2) != base.links.end()); EXPECT_EQ(linkId2, base.links.find(linkId2)->second->link->GetId()); - EXPECT_EQ(link2->GetName(), base.links.find(linkId2)->second->link->GetName()); + EXPECT_EQ( + link2->GetName(), base.links.find(linkId2)->second->link->GetName()); EXPECT_EQ(modelId2, base.childIdToParentId.find(linkId2)->second); // add collision shape box to link1 @@ -123,8 +128,11 @@ TEST(BaseClass, AddEntities) EXPECT_EQ(1u, link1->GetChildCount()); EXPECT_TRUE(base.collisions.find(boxId) != base.collisions.end()); - EXPECT_EQ(boxId, base.collisions.find(boxId)->second->collision->GetId()); - EXPECT_EQ(box->GetName(), base.collisions.find(boxId)->second->collision->GetName()); + EXPECT_EQ( + boxId, base.collisions.find(boxId)->second->collision->GetId()); + EXPECT_EQ( + box->GetName(), + base.collisions.find(boxId)->second->collision->GetName()); EXPECT_EQ(linkId1, base.childIdToParentId.find(boxId)->second); // add collision shape cylinder to link2 @@ -139,8 +147,12 @@ TEST(BaseClass, AddEntities) EXPECT_EQ(1u, link2->GetChildCount()); EXPECT_TRUE(base.collisions.find(cylinderId) != base.collisions.end()); - EXPECT_EQ(cylinderId, base.collisions.find(cylinderId)->second->collision->GetId()); - EXPECT_EQ(cylinder->GetName(), base.collisions.find(cylinderId)->second->collision->GetName()); + EXPECT_EQ( + cylinderId, + base.collisions.find(cylinderId)->second->collision->GetId()); + EXPECT_EQ( + cylinder->GetName(), + base.collisions.find(cylinderId)->second->collision->GetName()); EXPECT_EQ(linkId2, base.childIdToParentId.find(cylinderId)->second); // check indices diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index 0f192aa3a..bb5b589d3 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -72,7 +72,7 @@ Identity EntityManagementFeatures::GetWorld( { return this->GenerateIdentity(it->first, it->second); } - } + } } return this->GenerateInvalidId(); } @@ -376,15 +376,16 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) return worldIt->second->world->RemoveChildById(_modelID.id); } } - return false; + return false; } ///////////////////////////////////////////////// bool EntityManagementFeatures::ModelRemoved(const Identity &_modelID) const { if (this->models.find(_modelID.id) == this->models.end() - && this->childIdToParentId.find(_modelID.id) == this->childIdToParentId.end()) - return true; + && this->childIdToParentId.find(_modelID.id) == + this->childIdToParentId.end()) + return true; return false; } diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index a6ccc9ec7..bdc312eef 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -107,9 +107,6 @@ TEST(EntityManagement_TEST, RemoveEntities) EXPECT_EQ(nullptr, world->GetModel("empty model")); EXPECT_EQ(0ul, world->GetModelCount()); - // Why calling GetName shouldn't throw (from dartsim) - // EXPECT_EQ("empty model", model->GetName()); - auto model2 = world->ConstructEmptyModel("model2"); ASSERT_NE(nullptr, model2); EXPECT_EQ(0ul, model2->GetIndex()); diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc index b50e50e1d..84d0e781c 100644 --- a/tpe/plugin/src/FreeGroupFeatures.cc +++ b/tpe/plugin/src/FreeGroupFeatures.cc @@ -65,7 +65,7 @@ Identity FreeGroupFeatures::GetFreeGroupCanonicalLink( tpelib::Entity &link = model_it->second->model->GetCanonicalLink(); auto linkPtr = std::make_shared(); linkPtr->link = static_cast(&link); - return this->GenerateIdentity(link.GetId(), linkPtr); + return this->GenerateIdentity(link.GetId(), linkPtr); } return this->GenerateInvalidId(); } diff --git a/tpe/plugin/src/KinematicsFeatures.hh b/tpe/plugin/src/KinematicsFeatures.hh index 50f0b1585..d1bfade91 100644 --- a/tpe/plugin/src/KinematicsFeatures.hh +++ b/tpe/plugin/src/KinematicsFeatures.hh @@ -34,7 +34,8 @@ class KinematicsFeatures : public virtual Base, public virtual Implements3d { - public: FrameData3d FrameDataRelativeToWorld(const FrameID &_id) const override; + public: FrameData3d FrameDataRelativeToWorld( + const FrameID &_id) const override; }; } diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index 4da7aba1c..695b0ad62 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -93,7 +93,7 @@ Identity SDFFeatures::ConstructSdfLink( { ignwarn << "Model [" << _modelID.id << "] is not found" << std::endl; return this->GenerateInvalidId(); - } + } auto model = it->second->model; if (model == nullptr) { diff --git a/tpe/plugin/src/SDFFeatures_TEST.cc b/tpe/plugin/src/SDFFeatures_TEST.cc index 00d59e47c..9695080e6 100644 --- a/tpe/plugin/src/SDFFeatures_TEST.cc +++ b/tpe/plugin/src/SDFFeatures_TEST.cc @@ -19,6 +19,11 @@ #include +#include +#include + +#include + #include #include @@ -33,11 +38,6 @@ #include "lib/src/World.hh" #include "World.hh" -#include -#include - -#include - struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::tpeplugin::RetrieveWorld, ignition::physics::sdf::ConstructSdfLink, @@ -146,7 +146,8 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ(ignition::math::Pose3d(-0.275, 0, 1.1, 0, 0, 0), collision02.GetPose()); - ignition::physics::tpelib::Entity &link02 = model.GetChildByName("upper_link"); + ignition::physics::tpelib::Entity &link02 = + model.GetChildByName("upper_link"); ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), link02.GetId()); EXPECT_EQ("upper_link", link02.GetName()); @@ -179,7 +180,8 @@ TEST(SDFFeatures_TEST, CheckTpeData) EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0), collision05.GetPose()); - ignition::physics::tpelib::Entity &link03 = model.GetChildByName("lower_link"); + ignition::physics::tpelib::Entity &link03 = + model.GetChildByName("lower_link"); ASSERT_NE(ignition::physics::tpelib::Entity::kNullEntity.GetId(), link03.GetId()); EXPECT_EQ("lower_link", link03.GetName()); diff --git a/tpe/plugin/src/ShapeFeatures.cc b/tpe/plugin/src/ShapeFeatures.cc index f819b1f38..2fd585ad1 100644 --- a/tpe/plugin/src/ShapeFeatures.cc +++ b/tpe/plugin/src/ShapeFeatures.cc @@ -148,7 +148,7 @@ Identity ShapeFeatures::AttachCylinderShape( it->second->link->AddCollision()); collision.SetName(_name); collision.SetPose(math::eigen3::convert(_pose)); - + tpelib::CylinderShape cylindershape; cylindershape.SetRadius(_radius); cylindershape.SetLength(_height); diff --git a/tpe/plugin/src/SimulationFeatures.cc b/tpe/plugin/src/SimulationFeatures.cc index d686ed1a0..4dc700e49 100644 --- a/tpe/plugin/src/SimulationFeatures.cc +++ b/tpe/plugin/src/SimulationFeatures.cc @@ -39,7 +39,7 @@ void SimulationFeatures::WorldForwardStep( << std::endl; } std::shared_ptr world = it->second->world; - auto *dtDur = + auto *dtDur = _u.Query(); const double tol = 1e-6; if (dtDur) @@ -59,7 +59,8 @@ void SimulationFeatures::WorldForwardStep( std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { - // TODO(claireyywang): Implement contact points after collision detection is added + // TODO(anyone): + // Implement contact points after collision detection is added std::vector outContacts; std::shared_ptr world = this->worlds.at(_worldID)->world; return outContacts; diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index d66b895bc..7ebf40ff5 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -116,7 +116,7 @@ TEST_P(SimulationFeatures_TEST, StepWorld) for (const auto &world : worlds) { StepWorld(world, 1000); - + auto model = world->GetModel(0); ASSERT_NE(nullptr, model); auto link = model->GetLink(0); @@ -149,7 +149,7 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) "sphere2", 1.0, Eigen::Isometry3d::Identity()); EXPECT_EQ(2u, sphereLink->GetShapeCount()); EXPECT_EQ(sphere2, sphereLink->GetShape(1)); - + auto ground = world->GetModel("box"); auto groundLink = ground->GetLink(0); auto groundCollision = groundLink->GetShape(0); @@ -183,7 +183,7 @@ TEST_P(SimulationFeatures_TEST, ShapeFeatures) auto groundAABB = groundCollision->GetAxisAlignedBoundingBox(*groundCollision); - + auto cylinderAABB = cylinderCollision->GetAxisAlignedBoundingBox(*cylinderCollision); @@ -218,7 +218,8 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) ASSERT_NE(nullptr, freeGroup->CanonicalLink()); freeGroup->SetWorldPose( - ignition::math::eigen3::convert(ignition::math::Pose3d(0, 0, 2, 0, 0, 0))); + ignition::math::eigen3::convert( + ignition::math::Pose3d(0, 0, 2, 0, 0, 0))); freeGroup->SetWorldLinearVelocity( ignition::math::eigen3::convert(ignition::math::Vector3d(0.5, 0, 0.1))); freeGroup->SetWorldAngularVelocity(