Skip to content
Snippets Groups Projects
Commit b6ea1757 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Geometry] Removed useless const and &

parent 9eee31ac
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/geometry.hpp
+ 46
− 46
View file @ b6ea1757
......@@ -42,16 +42,16 @@ namespace se3
class IsSameCollisionPair
{
typedef Model::Index Index;
typedef std::pair < Index, Index > CollisionPair;
typedef std::pair < Index, Index > CollisionPair_t;
public:
IsSameCollisionPair( CollisionPair pair): _pair(pair) {}
IsSameCollisionPair( CollisionPair_t pair): _pair(pair) {}
bool operator()(CollisionPair pair) const
bool operator()(CollisionPair_t pair) const
{
return (pair == _pair);
}
private:
CollisionPair _pair;
CollisionPair_t _pair;
};
// Result of distance computation between two CollisionObject.
......@@ -60,19 +60,19 @@ namespace se3
typedef Model::Index Index;
DistanceResult(fcl::DistanceResult dist_fcl, Index o1, Index o2)
: fcl(dist_fcl), object1(o1), object2(o2)
: fcl_distance_result(dist_fcl), object1(o1), object2(o2)
{}
// Get distance between objects
const double & distance () const { return fcl.min_distance; }
double distance () const { return fcl_distance_result.min_distance; }
// Get closest point on inner object in global frame,
const Eigen::Vector3d closestPointInner () const { return toVector3d(fcl.nearest_points [0]); }
Eigen::Vector3d closestPointInner () const { return toVector3d(fcl_distance_result.nearest_points [0]); }
// Get closest point on outer object in global frame,
const Eigen::Vector3d closestPointOuter () const { return toVector3d(fcl.nearest_points [1]); }
Eigen::Vector3d closestPointOuter () const { return toVector3d(fcl_distance_result.nearest_points [1]); }
fcl::DistanceResult fcl;
fcl::DistanceResult fcl_distance_result;
std::size_t object1;
std::size_t object2;
}; // struct DistanceResult
......@@ -85,11 +85,11 @@ namespace se3
Index ngeom;
std::vector<fcl::CollisionObject> collision_objects;
std::vector<std::string> geom_names;
std::vector<Index> geom_parents; // Joint parent of body <i>, denoted <li> (li==parents[i])
std::vector<SE3> geometryPlacement; // Position of geometry object in parent joint's frame
std::vector<Index> geom_parents; // Joint parent of body <i>, denoted <li> (li==parents[i])
std::vector<SE3> geometryPlacement; // Position of geometry object in parent joint's frame
std::map < Index, std::list<Index> > innerObjects; // Associate a list of CollisionObjects to a given joint Id
std::map < Index, std::list<Index> > outerObjects; // Associate a list of CollisionObjects to a given joint Id
std::map < Index, std::list<Index> > innerObjects; // Associate a list of CollisionObjects to a given joint Id
std::map < Index, std::list<Index> > outerObjects; // Associate a list of CollisionObjects to a given joint Id
GeometryModel()
: ngeom(0)
......@@ -109,8 +109,8 @@ namespace se3
bool existGeomName( const std::string & name ) const;
const std::string& getGeomName( Index index ) const;
void addInnerObject(const Index& joint, const Index& inner_object);
void addOutterObject(const Index& joint, const Index& outer_object);
void addInnerObject(Index joint, Index inner_object);
void addOutterObject(Index joint, Index outer_object);
friend std::ostream& operator<<(std::ostream& os, const GeometryModel& model_geom);
......@@ -122,7 +122,7 @@ namespace se3
{
public:
typedef Model::Index Index;
typedef std::pair < Index, Index > CollisionPair;
typedef std::pair < Index, Index > CollisionPair_t;
Data& data_ref;
GeometryModel& model_geom;
......@@ -130,8 +130,8 @@ namespace se3
std::vector<se3::SE3> oMg;
std::vector<fcl::Transform3f> oMg_fcl;
std::vector < CollisionPair > collision_pairs;
std::vector < DistanceResult > distances_;
std::vector < CollisionPair_t > collision_pairs;
std::vector < DistanceResult > distances;
GeometryData(Data& data, GeometryModel& model_geom)
: data_ref(data)
......@@ -139,31 +139,31 @@ namespace se3
, oMg(model_geom.ngeom)
, oMg_fcl(model_geom.ngeom)
, collision_pairs()
, distances_()
, distances()
{
}
~GeometryData() {};
void addCollisionPair (const Index& co1, const Index& co2);
void addCollisionPair (const CollisionPair& pair);
void removeCollisionPair (const Index& co1, const Index& co2);
void removeCollisionPair (const CollisionPair& pair);
bool isCollisionPair (const Index& co1, const Index& co2);
bool isCollisionPair (const CollisionPair& pair);
void addCollisionPair (Index co1, Index co2);
void addCollisionPair (const CollisionPair_t& pair);
void removeCollisionPair (Index co1, Index co2);
void removeCollisionPair (const CollisionPair_t& pair);
bool isCollisionPair (Index co1, Index co2);
bool isCollisionPair (const CollisionPair_t& pair);
void fillAllPairsAsCollisions();
bool collide(const Index& co1, const Index& co2);
bool collide(Index co1, Index co2);
bool isColliding();
fcl::DistanceResult computeDistance(const Index& co1, const Index& co2);
fcl::DistanceResult computeDistance(Index co1, Index co2);
void computeDistances ();
std::vector < DistanceResult > distanceResults(); //TODO : to keep or not depending of public or not for distances_
std::vector < DistanceResult > distanceResults(); //TODO : to keep or not depending of public or not for distances
void displayCollisionPairs()
{
for (std::vector<CollisionPair>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
{
std::cout << it-> first << "\t" << it->second << std::endl;
}
......@@ -204,12 +204,12 @@ namespace se3
return geom_names[index];
}
inline void GeometryModel::addInnerObject(const Index& joint, const Index& inner_object)
inline void GeometryModel::addInnerObject(Index joint, Index inner_object)
{
innerObjects[joint].push_back(inner_object);
}
inline void GeometryModel::addOutterObject(const Index& joint, const Index& outer_object)
inline void GeometryModel::addOutterObject(Index joint, Index outer_object)
{
outerObjects[joint].push_back(outer_object);
}
......@@ -238,32 +238,32 @@ namespace se3
return os;
}
inline void GeometryData::addCollisionPair (const Index& co1, const Index& co2)
inline void GeometryData::addCollisionPair (Index co1, Index co2)
{
assert ( co1 < co2);
assert ( co2 < model_geom.ngeom);
CollisionPair pair(co1, co2);
CollisionPair_t pair(co1, co2);
addCollisionPair(pair);
}
inline void GeometryData::addCollisionPair (const CollisionPair& pair)
inline void GeometryData::addCollisionPair (const CollisionPair_t& pair)
{
assert(pair.first < pair.second);
assert(pair.second < model_geom.ngeom);
collision_pairs.push_back(pair);
}
inline void GeometryData::removeCollisionPair (const Index& co1, const Index& co2)
inline void GeometryData::removeCollisionPair (Index co1, Index co2)
{
assert(co1 < co2);
assert(co2 < model_geom.ngeom);
assert(isCollisionPair(co1,co2));
removeCollisionPair (CollisionPair(co1,co2));
removeCollisionPair (CollisionPair_t(co1,co2));
}
inline void GeometryData::removeCollisionPair (const CollisionPair& pair)
inline void GeometryData::removeCollisionPair (const CollisionPair_t& pair)
{
assert(pair.first < pair.second);
assert(pair.second < model_geom.ngeom);
......@@ -272,12 +272,12 @@ namespace se3
collision_pairs.erase(std::remove(collision_pairs.begin(), collision_pairs.end(), pair), collision_pairs.end());
}
inline bool GeometryData::isCollisionPair (const Index& co1, const Index& co2)
inline bool GeometryData::isCollisionPair (Index co1, Index co2)
{
return isCollisionPair(CollisionPair(co1,co2));
return isCollisionPair(CollisionPair_t(co1,co2));
}
inline bool GeometryData::isCollisionPair (const CollisionPair& pair)
inline bool GeometryData::isCollisionPair (const CollisionPair_t& pair)
{
return (std::find_if ( collision_pairs.begin(), collision_pairs.end(),
IsSameCollisionPair(pair)) != collision_pairs.end());
......@@ -295,7 +295,7 @@ namespace se3
}
}
inline bool GeometryData::collide(const Index& co1, const Index& co2)
inline bool GeometryData::collide(Index co1, Index co2)
{
fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
fcl::CollisionResult collisionResult;
......@@ -312,7 +312,7 @@ namespace se3
inline bool GeometryData::isColliding()
{
for (std::vector<CollisionPair>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
{
if (collide(it->first, it->second))
{
......@@ -322,7 +322,7 @@ namespace se3
return false;
}
inline fcl::DistanceResult GeometryData::computeDistance(const Index& co1, const Index& co2)
inline fcl::DistanceResult GeometryData::computeDistance(Index co1, Index co2)
{
fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
......@@ -334,10 +334,10 @@ namespace se3
inline void GeometryData::computeDistances ()
{
distances_.clear();
for (std::vector<CollisionPair>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
distances.clear();
for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
{
distances_.push_back( DistanceResult(computeDistance(it->first, it->second), it->first, it->second));
distances.push_back( DistanceResult(computeDistance(it->first, it->second), it->first, it->second));
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment