diff --git a/include/fcl/math/sampling.h b/include/fcl/math/sampling.h
index 61655e459c7242f14aa483a6026ff2ebfa3151fb..5ce22767499f917122162b01a7f7f782dadf068e 100644
--- a/include/fcl/math/sampling.h
+++ b/include/fcl/math/sampling.h
@@ -383,50 +383,25 @@ class SamplerSE3Euler_ball : public SamplerBase
public:
SamplerSE3Euler_ball() {}
- SamplerSE3Euler_ball(const Vec3f& c_,
- FCL_REAL r1, FCL_REAL r2,
- const Vec3f& c_ref)
+ SamplerSE3Euler_ball(FCL_REAL r_) : r(r_)
{
- setBound(c_, r1, r2, c_ref);
}
- SamplerSE3Euler_ball(const Vec3f& c_,
- FCL_REAL r_min_, FCL_REAL r_max_)
+ void setBound(const FCL_REAL& r_)
{
- setBound(c_, r_min_, r_max_);
+ r = r_;
}
-
- void setBound(const Vec3f& c_,
- FCL_REAL r1, FCL_REAL r2,
- const Vec3f& cref_)
- {
- c = c_;
- FCL_REAL cref_len = cref_.length();
- r_min = std::max<FCL_REAL>(cref_len - (r1 + r2), 0);
- r_max = cref_len + r1 + r2;
- }
-
- void setBound(const Vec3f& c_,
- FCL_REAL r_min_, FCL_REAL r_max_)
- {
- c = c_;
- r_min = r_min_;
- r_max = r_max_;
- }
-
- void getBound(Vec3f& c_,
- FCL_REAL& r_min_, FCL_REAL& r_max_) const
+
+ void getBound(FCL_REAL& r_) const
{
- c_ = c;
- r_min_ = r_min;
- r_max_ = r_max;
+ r_ = r;
}
Vecnf<6> sample() const
{
Vecnf<6> q;
FCL_REAL x, y, z;
- rng.ball(r_min, r_max, x, y, z);
+ rng.ball(0, r, x, y, z);
q[0] = x;
q[1] = y;
q[2] = z;
@@ -445,8 +420,8 @@ public:
}
protected:
- Vec3f c;
- FCL_REAL r_min, r_max;
+ FCL_REAL r;
+
};
@@ -455,50 +430,24 @@ class SamplerSE3Quat_ball : public SamplerBase
public:
SamplerSE3Quat_ball() {}
- SamplerSE3Quat_ball(const Vec3f& c_,
- FCL_REAL r1, FCL_REAL r2,
- const Vec3f& c_ref)
- {
- setBound(c_, r1, r2, c_ref);
- }
-
- SamplerSE3Quat_ball(const Vec3f& c_,
- FCL_REAL r_min_, FCL_REAL r_max_)
- {
- setBound(c_, r_min_, r_max_);
- }
-
- void setBound(const Vec3f& c_,
- FCL_REAL r1, FCL_REAL r2,
- const Vec3f& cref_)
- {
- c = c_;
- FCL_REAL cref_len = cref_.length();
- r_min = std::max<FCL_REAL>(cref_len - (r1 + r2), 0);
- r_max = cref_len + r1 + r2;
- }
+ SamplerSE3Quat_ball(FCL_REAL r_) : r(r_)
+ {}
- void setBound(const Vec3f& c_,
- FCL_REAL r_min_, FCL_REAL r_max_)
+ void setBound(const FCL_REAL& r_)
{
- c = c_;
- r_min = r_min_;
- r_max = r_max_;
+ r = r_;
}
- void getBound(Vec3f& c_,
- FCL_REAL& r_min_, FCL_REAL& r_max_) const
+ void getBound(FCL_REAL& r_) const
{
- c_ = c;
- r_min_ = r_min;
- r_max_ = r_max;
+ r_ = r;
}
Vecnf<7> sample() const
{
Vecnf<7> q;
FCL_REAL x, y, z;
- rng.ball(r_min, r_max, x, y, z);
+ rng.ball(0, r, x, y, z);
q[0] = x;
q[1] = y;
q[2] = z;
@@ -514,8 +463,7 @@ public:
}
protected:
- Vec3f c;
- FCL_REAL r_min, r_max;
+ FCL_REAL r;
};
diff --git a/src/penetration_depth.cpp b/src/penetration_depth.cpp
index e73fba0c0613f7fdc64a8b14853b0e5858ac75c7..3eed91ce5425d2c88a203bbd5409c5d84a01a182 100644
--- a/src/penetration_depth.cpp
+++ b/src/penetration_depth.cpp
@@ -89,44 +89,25 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
-
- FCL_REAL r = r1 + r2 + c1.length() + c2.length() + margin;
+
+ FCL_REAL r = r1 + r2 + margin;
sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
-
+
std::vector<Item<6> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
{
Vecnf<6> q = sampler.sample();
-
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
- Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
-
+ Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
o2->getCollisionGeometry(), tf, request, result);
- data[i] = Item<6>(q, (n_collide > 0));
+ data[i] = Item<6>(q, (n_collide > 0));
}
-
- /*
- Vecnf<6> scaler_lower_bound, scaler_upper_bound;
- for(std::size_t i = 0; i < 3; ++i)
- {
- scaler_lower_bound[i] = -r;
- scaler_upper_bound[i] = r;
- }
-
- for(std::size_t i = 3; i < 6; ++i)
- {
- scaler_lower_bound[i] = -boost::math::constants::pi<FCL_REAL>();
- scaler_upper_bound[i] = boost::math::constants::pi<FCL_REAL>();
- }
-
- classifier.setScaler(Scaler<6>(scaler_lower_bound, scaler_upper_bound));
- */
classifier->setScaler(computeScaler(data));
@@ -139,9 +120,9 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
- support_transforms_negative.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
@@ -155,9 +136,8 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
-
- FCL_REAL r = r1 + r2 + c1.length() + c2.length() + margin;
+ FCL_REAL r = r1 + r2 + margin;
sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
std::vector<Item<7> > data(n_samples);
@@ -166,7 +146,7 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Vecnf<7> q = sampler.sample();
Quaternion3f quat(q[3], q[4], q[5], q[6]);
- Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+ Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
@@ -185,10 +165,11 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<7>& q = svs[i].q;
+ Quaternion3f quat(q[3], q[4], q[5], q[6]);
if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
- support_transforms_negative.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
@@ -203,7 +184,7 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
- sampler.setBound(c1, r1 + margin, r2 + margin, c2);
+ sampler.setBound(r1 + r2 + margin);
std::vector<Item<6> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
@@ -212,7 +193,7 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
- Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+ Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
@@ -234,9 +215,9 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
- support_transforms_negative.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
@@ -251,7 +232,7 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
- sampler.setBound(c1, r1 + margin, r2 + margin, c2);
+ sampler.setBound(r1 + r2 + margin);
std::vector<Item<7> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
@@ -259,8 +240,7 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
Vecnf<7> q = sampler.sample();
Quaternion3f quat(q[3], q[4], q[5], q[6]);
-
- Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+ Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
@@ -279,10 +259,11 @@ std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<7>& q = svs[i].q;
+ Quaternion3f quat(q[3], q[4], q[5], q[6]);
if(svs[i].label)
- support_transforms_positive.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
- support_transforms_negative.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
diff --git a/test/test_fcl_xmldata.cpp b/test/test_fcl_xmldata.cpp
index 5280e7e9be4c6d78a1f91f7ea6a58fc2683ac3d3..a0c44f81efa2e5ae3d60f9f4d914127601608a6e 100644
--- a/test/test_fcl_xmldata.cpp
+++ b/test/test_fcl_xmldata.cpp
@@ -74,7 +74,7 @@ static void loadSceneFile(const std::string& filename,
grid = grid->NextSiblingElement("GRID");
}
- std::cout << "#vertices " << n_vertices << std::endl;
+ // std::cout << "#vertices " << n_vertices << std::endl;
TiXmlElement* tri = object->FirstChildElement("TRIA");
int n_tris = 0;
@@ -104,7 +104,7 @@ static void loadSceneFile(const std::string& filename,
tri = tri->NextSiblingElement("TRIA");
}
- std::cout << "#triangles " << n_tris << std::endl;
+ // std::cout << "#triangles " << n_tris << std::endl;
if(object_id - 1 == (int)points_array.size())
{
@@ -128,7 +128,7 @@ static void loadSceneFile(const std::string& filename,
i++;
}
- std::cout << "#objects " << i << std::endl;
+ // std::cout << "#objects " << i << std::endl;
}
motion = doc.FirstChildElement("MOTION");
@@ -181,7 +181,7 @@ static void loadSceneFile(const std::string& filename,
n_frame++;
}
- std::cout << "#frames " << n_frame << std::endl;
+ // std::cout << "#frames " << n_frame << std::endl;
}
}
else
@@ -329,11 +329,12 @@ BOOST_AUTO_TEST_CASE(scene_test_penetration)
RNG::setSeed(1);
boost::filesystem::path path(TEST_RESOURCES_DIR);
- /*
- std::cout << "manyframes/Model_4" << std::endl;
- std::string filename0 = (path / "manyframes/Model_4.xml").string();
+ std::cout << "manyframes/Model_5" << std::endl;
+ std::string filename0 = (path / "manyframes/Model_5.xml").string();
scenePenetrationTest(filename0);
- */
+
+ return;
+
std::cout << "scenario-1-2-3/Model_1_Scenario_1" << std::endl;
std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();