Skip to content

Commit

Permalink
Pass test_fcl_broadphase
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 committed Jul 27, 2016
1 parent 04f8e7d commit 849d37e
Show file tree
Hide file tree
Showing 4 changed files with 65 additions and 82 deletions.
30 changes: 18 additions & 12 deletions include/fcl/BV/AABB.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,21 +82,25 @@ class AABB
/// @brief Check whether two AABB are overlap
inline bool overlap(const AABB& other) const
{
if(min_[0] > other.max_[0]) return false;
if(min_[1] > other.max_[1]) return false;
if(min_[2] > other.max_[2]) return false;
if ((min_.array() > other.max_.array()).any())
return false;

if(max_[0] < other.min_[0]) return false;
if(max_[1] < other.min_[1]) return false;
if(max_[2] < other.min_[2]) return false;
if ((max_.array() < other.min_.array()).any())
return false;

return true;
}

/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
{
return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
if ((min_.array() > other.min_.array()).any())
return false;

if ((max_.array() < other.max_.array()).any())
return false;

return true;
}


Expand All @@ -118,18 +122,20 @@ class AABB
return false;
}

overlap_part.min_ = min_.cwiseMin(other.min_);
overlap_part.max_ = max_.cwiseMax(other.max_);
overlap_part.min_ = min_.cwiseMax(other.min_);
overlap_part.max_ = max_.cwiseMin(other.max_);
return true;
}


/// @brief Check whether the AABB contains a point
inline bool contain(const Vec3f& p) const
{
if(p[0] < min_[0] || p[0] > max_[0]) return false;
if(p[1] < min_[1] || p[1] > max_[1]) return false;
if(p[2] < min_[2] || p[2] > max_[2]) return false;
if ((min_.array() > p.array()).any())
return false;

if ((max_.array() < p.array()).any())
return false;

return true;
}
Expand Down
47 changes: 13 additions & 34 deletions src/shape/geometric_shapes_utility.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -461,79 +461,58 @@ void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
template<>
void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.linear();
const Vec3f& T = tf.translation();

bv.To = T;
bv.axis = R;
bv.To = tf.translation();
bv.axis = tf.linear();
bv.extent = s.side * (FCL_REAL)0.5;
}

template<>
void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
{
const Vec3f& T = tf.translation();

bv.To = T;
bv.To = tf.translation();
bv.axis.setIdentity();
bv.extent.setConstant(s.radius);
}

template<>
void computeBV<OBB, Ellipsoid>(const Ellipsoid& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.linear();
const Vec3f& T = tf.translation();

bv.To = T;
bv.axis = R;
bv.To = tf.translation();
bv.axis = tf.linear();
bv.extent = s.radii;
}

template<>
void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.linear();
const Vec3f& T = tf.translation();

bv.To = T;
bv.axis = R;
bv.To = tf.translation();
bv.axis = tf.linear();
bv.extent << s.radius, s.radius, s.lz / 2 + s.radius;
}

template<>
void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.linear();
const Vec3f& T = tf.translation();

bv.To = T;
bv.axis = R;
bv.To = tf.translation();
bv.axis = tf.linear();
bv.extent << s.radius, s.radius, s.lz / 2;
}

template<>
void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.linear();
const Vec3f& T = tf.translation();

bv.To = T;
bv.axis = R;
bv.To = tf.translation();
bv.axis = tf.linear();
bv.extent << s.radius, s.radius, s.lz / 2;
}

template<>
void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.linear();
const Vec3f& T = tf.translation();

fit(s.points, s.num_points, bv);

bv.axis = R;

bv.To = R * bv.To + T;
bv.axis = tf.linear();
bv.To = tf * bv.To;
}

template<>
Expand Down
69 changes: 33 additions & 36 deletions test/test_fcl_broadphase.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -318,7 +318,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_sca
for(std::size_t i = 0; i < n; ++i)
{
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, box, Transform3f());
generateBVHModel(*model, box, Transform3f::Identity());
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model), transforms[i]));
}

Expand All @@ -327,7 +327,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_sca
for(std::size_t i = 0; i < n; ++i)
{
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, sphere, Transform3f(), 16, 16);
generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model), transforms[i]));
}

Expand All @@ -336,7 +336,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_sca
for(std::size_t i = 0; i < n; ++i)
{
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model), transforms[i]));
}
}
Expand All @@ -358,9 +358,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double

Box* box = new Box(single_size, single_size, single_size);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(box),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -371,9 +371,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double

Sphere* sphere = new Sphere(single_size / 2);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(sphere),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -384,9 +384,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double

Ellipsoid* ellipsoid = new Ellipsoid(single_size / 2, single_size / 2, single_size / 2);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(ellipsoid),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -397,9 +397,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double

Cylinder* cylinder = new Cylinder(single_size / 2, single_size);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(cylinder),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -410,9 +410,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double

Cone* cone = new Cone(single_size / 2, single_size);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(cone),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}
}

Expand All @@ -433,11 +433,11 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, do

Box box(single_size, single_size, single_size);
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, box, Transform3f());
generateBVHModel(*model, box, Transform3f::Identity());
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -448,11 +448,11 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, do

Sphere sphere(single_size / 2);
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, sphere, Transform3f(), 16, 16);
generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -463,11 +463,11 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, do

Ellipsoid ellipsoid(single_size / 2, single_size / 2, single_size / 2);
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, ellipsoid, Transform3f(), 16, 16);
generateBVHModel(*model, ellipsoid, Transform3f::Identity(), 16, 16);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -478,11 +478,11 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, do

Cylinder cylinder(single_size / 2, single_size);
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}

for(; i < n_edge * n_edge * n_edge / 4; ++i)
Expand All @@ -493,11 +493,11 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, do

Cone cone(single_size / 2, single_size);
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
generateBVHModel(*model, cone, Transform3f(), 16, 16);
generateBVHModel(*model, cone, Transform3f::Identity(), 16, 16);
env.push_back(new CollisionObject(std::shared_ptr<CollisionGeometry>(model),
Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
Transform3f(Eigen::Translation3d(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
z * step_size + delta_size + 0.5 * single_size - env_scale)))));
}
}

Expand Down Expand Up @@ -1044,16 +1044,13 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s
FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;

Quaternion3f q1, q2, q3;
q1.fromAxisAngle(Vec3f(1, 0, 0), rand_angle_x);
q2.fromAxisAngle(Vec3f(0, 1, 0), rand_angle_y);
q3.fromAxisAngle(Vec3f(0, 0, 1), rand_angle_z);
Quaternion3f q = q1 * q2 * q3;
Matrix3f dR;
q.toRotation(dR);
Matrix3f dR(
Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX())
* Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY())
* Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);

Matrix3f R = env[i]->linear();
Matrix3f R = env[i]->getRotation();
Vec3f T = env[i]->getTranslation();
env[i]->setTransform(dR * R, dR * T + dT);
env[i]->computeAABB();
Expand Down
1 change: 1 addition & 0 deletions test/test_fcl_utility.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -260,6 +260,7 @@ void generateRandomTransforms(FCL_REAL extents[6], std::vector<Transform3f>& tra
Matrix3f R;
eulerToMatrix(a, b, c, R);
Vec3f T(x, y, z);
transforms[i].setIdentity();
transforms[i].linear() = R;
transforms[i].translation() = T;
}
Expand Down

0 comments on commit 849d37e

Please sign in to comment.