Commit 28e4555e authored by t steve's avatar t steve
Browse files

affordance runs with hpp-pinocchio

parent f4a920fd
*~
/build/*
*.swp
*.user
/build-rel/*
......@@ -93,7 +93,7 @@ namespace hpp {
/// \param nromal Normal vector of the tested triangle.
bool requirement (const fcl::Vec3f& normal)
{
return ((zWorld_ - normal).sqrLength() < margin_);
return ((zWorld_ - normal).squaredNorm() < margin_);
}
}; // class SupportOperation
/// Class that contains the information needed to create affordance
......@@ -138,9 +138,10 @@ namespace hpp {
explicit Support45Operation (const double margin = 0.3, const double nbTriMargin = 0.3,
const double minArea = 0.05,
const char* affordanceName = "Support45"):
OperationBase(margin, 0.05, minArea, affordanceName) {}
OperationBase(margin, 0.05, minArea, affordanceName)
, axis45_ (fcl::Vec3f(1./sqrt(2.),0,1./sqrt(2.))) {}
private:
const fcl::Vec3f axis45_ = fcl::Vec3f(1./sqrt(2.),0,1./sqrt(2.));
const fcl::Vec3f axis45_;
/// The implementation of the requirement function for Support45 affordances
/// overrides the virtual function in class OperationBase.
......@@ -150,7 +151,7 @@ namespace hpp {
{
fcl::Vec3f projectedNormal(0,0,normal[2]); // project normal in x,z plan
projectedNormal[0] = sqrt(normal[0]*normal[0] + normal[1]*normal[1]);
return ((axis45_ - projectedNormal).sqrLength() < margin_);
return ((axis45_ - projectedNormal).squaredNorm() < margin_);
}
}; // class LeanOperation
......
......@@ -57,11 +57,11 @@ namespace hpp {
refPoints.push_back(refTri.points.p3);
for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
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) {
if ((refPoints[vertIdx] - searchTri.points.p1).squaredNorm () < margin
|| (refPoints[vertIdx] - searchTri.points.p2).squaredNorm () < margin
|| (refPoints[vertIdx] - searchTri.points.p3).squaredNorm () < margin) {
if (refOp->requirement (searchTri.normal)) {
if ((searchTri.normal - refTri.normal).sqrLength () < refOp->neighbouringTriangleMargin_) {
if ((searchTri.normal - refTri.normal).squaredNorm () < refOp->neighbouringTriangleMargin_) {
area += searchTri.area;
listPotential.push_back (searchIdx);
searchLinkedTriangles (listPotential, refOp, allTris,
......@@ -259,7 +259,7 @@ namespace hpp {
if((id_lastPoint == id_pointOnHull) || (isLeft(points[id_pointOnHull], points[id_lastPoint],points[id_current]) > 0)
|| ((id_current!=id_pointOnHull) && (id_current!= id_lastPoint) &&
(isLeft(points[id_pointOnHull], points[id_lastPoint],points[id_current]) == 0 )
&& ((points[id_pointOnHull]-points[id_current]).sqrLength() < (points[id_pointOnHull]-points[id_lastPoint]).sqrLength()))){ // if on the same line, take the closest one from ptsOnHull
&& ((points[id_pointOnHull]-points[id_current]).squaredNorm() < (points[id_pointOnHull]-points[id_lastPoint]).squaredNorm()))){ // if on the same line, take the closest one from ptsOnHull
if( ( std::find(res.begin(),res.end(),id_current) == res.end() ) || ((res.size()>0) && (id_current == res[0])))// only selected it if not on the list (or the first)
id_lastPoint = id_current;
}
......@@ -350,9 +350,9 @@ namespace hpp {
else
next=vertices[orderedIndex[id+1]];
A=(next-vertices[orderedIndex[id]]).normalize();
B=(prev-vertices[orderedIndex[id]]).normalize();
dir=(A+B).normalize();
A=(next-vertices[orderedIndex[id]]).normalized();
B=(prev-vertices[orderedIndex[id]]).normalized();
dir=(A+B).normalized();
/* if(dir.length() < std::numeric_limits<double>::epsilon()){ // this mean that prev, current and next are aligned, we make a 90° angle
dir=
}*/
......
......@@ -53,7 +53,7 @@ BOOST_AUTO_TEST_CASE (one_triangle2)
fcl::Matrix3f R;
fcl::Quaternion3f quat (1,0,0,0);
quat.toRotation(R);
R = quat.matrix();
fcl::Vec3f T (0,0,0);
fcl::Transform3f pose (R, T);
......
......@@ -58,10 +58,8 @@ BOOST_AUTO_TEST_CASE (oriented_triangles1)
boost::shared_ptr<Model> model (new Model ());
fcl::Triangle tri (0,1,2);
triangles.push_back (tri);
fcl::Quaternion3f quat;
quat.fromAxisAngle(axis,3.1416*float(5*i)/180.0);
quat.toRotation(R);
R = Eigen::AngleAxisd(3.1416*double(5*i)/180.0, axis);
fcl::Transform3f pose (R, T);
......
......@@ -57,11 +57,9 @@ BOOST_AUTO_TEST_CASE (oriented_triangles2)
for (int i = 0; i < size_; ++i) {
boost::shared_ptr<Model> model (new Model ());
fcl::Triangle tri (0,1,2);
triangles.push_back (tri);
fcl::Quaternion3f quat;
quat.fromAxisAngle(axis,3.1416*float(i)/180.0);
quat.toRotation(R);
triangles.push_back (tri);
R = Eigen::AngleAxisd(3.1416*double(i)/180.0, axis);
fcl::Transform3f pose (R, T);
......
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