Commit e37d062e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

comments

parent 7e49de09
/build/*
*.swp
*.user
......@@ -34,6 +34,7 @@ namespace hpp {
{
Triangle () {}
//STEVE: are you sure you want to do a copy of the fcl triangle
Triangle (const fcl::Triangle& inFclTri, const TrianglePoints& inPoints):
points (inPoints), fclTri (new fcl::Triangle (inFclTri))
{
......@@ -61,7 +62,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
/// \{
......@@ -93,18 +94,25 @@ namespace hpp {
class SemanticsData
{
public:
//STEVE Delegate constructors are c++ 11 only. For the time being we must remain in c ++ 10
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)
// STEVE: probably useless to call resize here, so I'd stick with the default constructor.
SemanticsData (const long unsigned int affordanceCount)
// STEVE Taking refs of numbers is not desirable because it does not allow dynamic initlization
// SemanticsData(2) is not possible, you need to do int tmp = 2; SemanticsData(tmp)
{
affordances_.resize(affordanceCount);
}
// STEVE typedef ?
std::vector<std::vector<AffordancePtr_t> > affordances_;
private:
SemanticsData(const SemanticsData&); // Prevent copy-construction
SemanticsData& operator=(const SemanticsData&);
};
......@@ -131,7 +139,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,
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,
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>
......@@ -41,12 +41,11 @@ namespace hpp {
const unsigned int& refTriIdx, double& area)
{
// TODO: think of a better way of declaring margins?
// STEVE: create a struct Configuration with all those params, and use it as a parameter
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]; // STEVE no need to copy object
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) {
......@@ -70,9 +69,8 @@ namespace hpp {
searchableTris, searchIdx, area);
}
} else {
// if linked face does not fulfil global requirement, discard
std::remove (searchableTris.begin (), searchableTris.end (), searchIdx);
searchableTris.pop_back ();
// if linked face does not fulfill global requirement, discard
searchableTris.erase(std::remove(searchableTris.begin(), searchableTris.end(), searchIdx), searchableTris.end());
}
break; // jump out of vertex loop if triangle already tested for affordance
}
......@@ -91,7 +89,7 @@ namespace hpp {
std::vector<std::vector<unsigned int> > potentialAffordances (opVec.size ());
SemanticsDataPtr_t foundAffordances(new SemanticsData(opVec.size ()));
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!!
......
......@@ -22,11 +22,23 @@
#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)
{
hpp::affordance::SupportOperationPtr_t support (new hpp::affordance::SupportOperation(0.3));
hpp::affordance::SupportOperationPtr_t support (new hpp::affordance::SupportOperation(0.3));
hpp::affordance::LeanOperationPtr_t lean (new hpp::affordance::LeanOperation(0.1));
std::vector <hpp::affordance::OperationBasePtr_t> operations;
......@@ -35,10 +47,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_triangles)
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);
......
Markdown is supported
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