Commit 6d0dff26 authored by Akseppal's avatar Akseppal
Browse files

clean up code

parent cde7309a
......@@ -61,7 +61,7 @@ namespace hpp {
};
// helper function to extract mesh model of an fcl::collisionObstacle
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object);
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object);
/// \addtogroup affordance
/// \{
......@@ -94,17 +94,11 @@ namespace hpp {
{
public:
SemanticsData (): SemanticsData (0) {}
/// Constructor that adjust the affordances_ vector to
/// the amount of defined affordance types but leaves the
/// vectors of affordances empty.
///
/// \param affordanceCount number of defined affordance types.
SemanticsData (const long unsigned int& affordanceCount)
{
affordances_.resize(affordanceCount);
}
SemanticsData () {}
std::vector<std::vector<AffordancePtr_t> > affordances_;
private:
SemanticsData(const SemanticsData&); // Prevent copy-construction
SemanticsData& operator=(const SemanticsData&);
};
......@@ -131,7 +125,7 @@ namespace hpp {
/// the global requirement set by the affordance type, it is deleted
/// and will not be tested again in subsequent recursive steps.
/// \param refTriIdx index corresponding to the last found triangle that
/// fullfils bot the local and the global requirement. It is then
/// fullfils both the local and the global requirement. It is then
/// used as reference in the following recursive step.
/// \param area total area of all triangles that are part of the potential
/// affordance object. Every time a triangle fulfilling all set
......
......@@ -29,10 +29,10 @@ namespace hpp {
public:
OperationBase (): zWorld_(0,0,1), margin_(0.3), minArea_(0.05),
affordance_("noAffordance") {}
explicit OperationBase (const double & margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "noAffordance"): zWorld_(0,0,1),
explicit OperationBase (const double margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "noAffordance"): zWorld_(0,0,1),
margin_(margin), minArea_(minArea), affordance_(affordanceName) {}
virtual bool requirement (fcl::Vec3f normal) =0;
virtual bool requirement (const fcl::Vec3f& normal) =0;
const fcl::Vec3f zWorld_;
const double margin_;
......@@ -43,31 +43,25 @@ namespace hpp {
class SupportOperation : public OperationBase
{
public:
explicit SupportOperation (const double & margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "Support"):
explicit SupportOperation (const double margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "Support"):
OperationBase(margin, minArea, affordanceName) {}
bool requirement (fcl::Vec3f normal)
bool requirement (const fcl::Vec3f& normal)
{
if ((zWorld_ - normal).sqrLength() < margin_) {
return true;
}
return false;
return ((zWorld_ - normal).sqrLength() < margin_);
}
}; // class SupportOperation
class LeanOperation : public OperationBase
{
public:
explicit LeanOperation (const double & margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "Lean"):
explicit LeanOperation (const double margin = 0.3, const double minArea = 0.05,
const char* affordanceName = "Lean"):
OperationBase(margin, minArea, affordanceName) {}
bool requirement (fcl::Vec3f normal)
bool requirement (const fcl::Vec3f& normal)
{
if (fabs (normal.dot(zWorld_)) < margin_) {
return true;
}
return false;
return (fabs (normal.dot(zWorld_)) < margin_);
}
}; // class LeanOperation
......
......@@ -27,7 +27,7 @@
namespace hpp {
namespace affordance {
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t object)
BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object)
{
assert (object->collisionGeometry ()->getNodeType () == fcl::BV_OBBRSS);
const BVHModelOBConst_Ptr_t model = boost::static_pointer_cast<const BVHModelOB>
......@@ -43,10 +43,9 @@ namespace hpp {
// TODO: think of a better way of declaring margins?
const double marginRad = 0.3;
const double margin = 1e-15;
Triangle refTri = allTris[refTriIdx];
// TODO: find a cleaner way of removing & resizing the searchableTriangels vector?
std::remove (searchableTris.begin (), searchableTris.end (), refTriIdx);
searchableTris.pop_back ();
const Triangle& refTri = allTris[refTriIdx];
searchableTris.erase (std::remove(searchableTris.begin (),
searchableTris.end (), refTriIdx), searchableTris.end ());
for (unsigned int searchIdx = 0; searchIdx < allTris.size (); searchIdx++) {
std::vector<unsigned int>::iterator it = std::find (searchableTris.begin (),
searchableTris.end (), searchIdx);
......@@ -58,7 +57,7 @@ namespace hpp {
refPoints.push_back(refTri.points.p2);
refPoints.push_back(refTri.points.p3);
for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
Triangle searchTri = allTris [searchIdx];
const Triangle& searchTri = allTris [searchIdx];
if ((refPoints[vertIdx] - searchTri.points.p1).sqrLength () < margin
|| (refPoints[vertIdx] - searchTri.points.p2).sqrLength () < margin
|| (refPoints[vertIdx] - searchTri.points.p3).sqrLength () < margin) {
......@@ -71,8 +70,8 @@ namespace hpp {
}
} else {
// if linked face does not fulfil global requirement, discard
std::remove (searchableTris.begin (), searchableTris.end (), searchIdx);
searchableTris.pop_back ();
searchableTris.erase(std::remove(searchableTris.begin(),
searchableTris.end(), searchIdx), searchableTris.end());
}
break; // jump out of vertex loop if triangle already tested for affordance
}
......@@ -89,12 +88,10 @@ namespace hpp {
std::vector <unsigned int> unsetTriangles;
double totArea = .0;
std::vector<std::vector<unsigned int> > potentialAffordances (opVec.size ());
SemanticsDataPtr_t foundAffordances(new SemanticsData(opVec.size ()));
SemanticsDataPtr_t foundAffordances(new SemanticsData());
for(unsigned int i = 0; i < model->num_tris; ++i)
for(int i = 0; i < model->num_tris; ++i)
{
// TODO: make sure triagle points are correct in world frame after every
// change!!
TrianglePoints tri;
fcl::Triangle fcltri = model->tri_indices [i];
tri.p1 = colObj->getRotation () *
......
......@@ -22,6 +22,19 @@
#define BOOST_TEST_MODULE test-operations
#include <boost/test/included/unit_test.hpp>
const double epsilon = 10e-6;
bool compDouble(const double a, const double b)
{
return a-b < epsilon;
}
bool compVec(const fcl::Vec3f& a, const fcl::Vec3f& b)
{
return (a-b).norm() < epsilon;
}
BOOST_AUTO_TEST_SUITE (test_affordance)
BOOST_AUTO_TEST_CASE (operations)
......@@ -35,10 +48,10 @@ BOOST_AUTO_TEST_CASE (operations)
const fcl::Vec3f normal1(0, 0, 1);
BOOST_CHECK_MESSAGE (support->zWorld_ == normal1,
BOOST_CHECK_MESSAGE (compVec(support->zWorld_,normal1),
"default value for zWorld should be " << normal1 << " but is " << support->zWorld_);
BOOST_CHECK_MESSAGE (lean->margin_ == 0.1,
BOOST_CHECK_MESSAGE (compDouble(lean->margin_,0.1),
"margin should match the one given when creating operation");
BOOST_CHECK_MESSAGE (operations.size () == 2,
......
......@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE (oriented_triangles2)
fcl::Vec3f axis (1,0,0);
fcl::Matrix3f R;
for (unsigned int i = 0; i < size_; ++i) {
for (int i = 0; i < size_; ++i) {
boost::shared_ptr<Model> model (new Model ());
fcl::Triangle tri (0,1,2);
triangles.push_back (tri);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment