Commit e861655b authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Replace Box::side by Box::halfSide.

parent 8a69489a
......@@ -52,19 +52,19 @@ namespace fcl
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
{
double a = shape.side[0];
double b = shape.side[1];
double c = shape.side[2];
double a = shape.halfSide[0];
double b = shape.halfSide[1];
double c = shape.halfSide[2];
std::vector<Vec3f> points(8);
std::vector<Triangle> tri_indices(12);
points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
points[0] = Vec3f ( a, -b, c);
points[1] = Vec3f ( a, b, c);
points[2] = Vec3f (-a, b, c);
points[3] = Vec3f (-a, -b, c);
points[4] = Vec3f ( a, -b, -c);
points[5] = Vec3f ( a, b, -c);
points[6] = Vec3f (-a, b, -c);
points[7] = Vec3f (-a, -b, -c);
tri_indices[0].set(0, 4, 1);
tri_indices[1].set(1, 4, 5);
......
......@@ -80,18 +80,18 @@ public:
class Box : public ShapeBase
{
public:
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z)
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2)
{
}
Box(const Vec3f& side_) : ShapeBase(), side(side_)
Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2)
{
}
Box() {}
/// @brief box side length
Vec3f side;
/// @brief box side half-length
Vec3f halfSide;
/// @brief Compute AABB
void computeLocalAABB();
......@@ -101,18 +101,14 @@ public:
FCL_REAL computeVolume() const
{
return side[0] * side[1] * side[2];
return 8*halfSide.prod();
}
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
FCL_REAL a2 = side[0] * side[0] * V;
FCL_REAL b2 = side[1] * side[1] * V;
FCL_REAL c2 = side[2] * side[2] * V;
return (Matrix3f() << (b2 + c2) / 12, 0, 0,
0, (a2 + c2) / 12, 0,
0, 0, (a2 + b2) / 12).finished();
Vec3f s (halfSide.cwiseAbs2() * V);
return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
}
};
......
......@@ -860,8 +860,8 @@ namespace fcl {
inline int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
inline int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1, const Vec3f& T1,
const Vec3f& halfSide2, const Matrix3f& R2, const Vec3f& T2,
Vec3f& normal, FCL_REAL* depth, int* return_code,
int maxc, std::vector<ContactPoint>& contacts)
{
......@@ -874,8 +874,8 @@ namespace fcl {
Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1
// get side lengths / 2
Vec3f A (side1 * 0.5);
Vec3f B (side2 * 0.5);
const Vec3f& A (halfSide1);
const Vec3f& B (halfSide2);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
Matrix3f R (R1.transpose() * R2);
......@@ -1443,8 +1443,8 @@ namespace fcl {
int return_code;
Vec3f normal;
FCL_REAL depth;
/* int cnum = */ boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(),
s2.side, tf2.getRotation(), tf2.getTranslation(),
/* int cnum = */ boxBox2(s1.halfSide, tf1.getRotation(), tf1.getTranslation(),
s2.halfSide, tf2.getRotation(), tf2.getTranslation(),
normal, &depth, &return_code,
4, contacts);
......@@ -1518,11 +1518,9 @@ namespace fcl {
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
Vec3f Q = R.transpose() * new_s2.n;
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
Vec3f B = A.cwiseAbs();
Vec3f Q (R.transpose() * new_s2.n);
FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
FCL_REAL depth = Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T);
return (depth >= 0);
}
......@@ -1538,27 +1536,17 @@ namespace fcl {
const Vec3f& T = tf1.getTranslation();
// Q: plane normal expressed in box frame
Vec3f Q = R.transpose() * new_s2.n;
const Vec3f Q (R.transpose() * new_s2.n);
// A: scalar products of each side with normal
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
Vec3f B = A.cwiseAbs();
const Vec3f A (Q.cwiseProduct(s1.halfSide));
distance = new_s2.signedDistance(T) - 0.5 * (B[0] + B[1] + B[2]);
distance = new_s2.signedDistance(T) - A.lpNorm<1>();
if(distance > 0) {
p1 = T;
for (Vec3f::Index i=0; i<3; ++i) {
p1 -= A [i] > 0 ? (-.5 * s1.side [i] * R.col (i)) :
(.5 * s1.side [i] * R.col (i));
}
p2 = p1 - distance * new_s2.n;
p1.noalias() = T + R * (A.array() > 0).select (s1.halfSide, - s1.halfSide);
p2.noalias() = p1 - distance * new_s2.n;
return false;
}
Vec3f axis[3];
axis[0] = R.col(0);
axis[1] = R.col(1);
axis[2] = R.col(2);
/// find deepest point
Vec3f p(T);
int sign = 0;
......@@ -1566,25 +1554,21 @@ namespace fcl {
if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
{
sign = (A[0] > 0) ? -1 : 1;
p += axis[0] * (0.5 * s1.side[0] * sign);
p += R.col(0) * (s1.halfSide[0] * sign);
}
else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
{
sign = (A[1] > 0) ? -1 : 1;
p += axis[1] * (0.5 * s1.side[1] * sign);
p += R.col(1) * (s1.halfSide[1] * sign);
}
else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
{
sign = (A[2] > 0) ? -1 : 1;
p += axis[2] * (0.5 * s1.side[2] * sign);
p += R.col(2) * (s1.halfSide[2] * sign);
}
else
{
for(std::size_t i = 0; i < 3; ++i)
{
sign = (A[i] > 0) ? -1 : 1;
p += axis[i] * (0.5 * s1.side[i] * sign);
}
p.noalias() += R * (A.array() > 0).select (-s1.halfSide, s1.halfSide);
}
/// compute the contact point from the deepest point
......@@ -2040,33 +2024,32 @@ namespace fcl {
// Vec3f* contact_points,
// FCL_REAL* penetration_depth, Vec3f* normal)
{
FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
static const FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
Plane new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
Vec3f Q = R.transpose() * new_s2.n;
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
Vec3f B = A.cwiseAbs();
const Vec3f Q (R.transpose() * new_s2.n);
const Vec3f A (Q.cwiseProduct(s1.halfSide));
FCL_REAL signed_dist = new_s2.signedDistance(T);
distance = std::abs(signed_dist) - 0.5 * (B[0] + B[1] + B[2]);
distance = std::abs(signed_dist) - A.lpNorm<1>();
if(distance > 0) {
// Is the box above or below the plane
int sign = signed_dist > 0 ? 1 : -1;
const bool positive = signed_dist > 0;
// Set p1 at the center of the box
p1 = T;
for (Vec3f::Index i=0; i<3; ++i) {
// scalar product between box axis and plane normal
FCL_REAL alpha (R.col (i).dot (new_s2.n));
if (sign * alpha > eps) {
p1 -= .5 * R.col (i) * s1.side [i];
} else if (sign * alpha < -eps) {
p1 += .5 * R.col (i) * s1.side [i];
FCL_REAL alpha ((positive ? 1 : -1 ) * R.col (i).dot (new_s2.n));
if (alpha > eps) {
p1 -= R.col (i) * s1.halfSide [i];
} else if (alpha < -eps) {
p1 += R.col (i) * s1.halfSide [i];
}
}
p2 = p1 - sign * distance * new_s2.n;
p2 = p1 - ( positive ? distance : -distance) * new_s2.n;
assert (new_s2.distance (p2) < 3 *eps);
return false;
}
......@@ -2087,32 +2070,25 @@ namespace fcl {
if(std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[0] > 0) ? -1 : 1;
sign2 *= sign;
p += axis[0] * (0.5 * s1.side[0] * sign2);
int sign2 = (A[0] > 0) ? -sign : sign;
p += R.col(0) * (s1.halfSide[0] * sign2);
}
else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[1] > 0) ? -1 : 1;
sign2 *= sign;
p += axis[1] * (0.5 * s1.side[1] * sign2);
int sign2 = (A[1] > 0) ? -sign : sign;
p += R.col(1) * (s1.halfSide[1] * sign2);
}
else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
{
int sign2 = (A[2] > 0) ? -1 : 1;
sign2 *= sign;
p += axis[2] * (0.5 * s1.side[2] * sign2);
int sign2 = (A[2] > 0) ? -sign : sign;
p += R.col(2) * (s1.halfSide[2] * sign2);
}
else
{
for(std::size_t i = 0; i < 3; ++i)
{
int sign2 = (A[i] > 0) ? -1 : 1;
sign2 *= sign;
p += axis[i] * (0.5 * s1.side[i] * sign2);
}
Vec3f tmp(sign * R * s1.halfSide);
p += (A.array() > 0).select (- tmp, tmp);
}
// compute the contact point by project the deepest point onto the plane
......@@ -2143,7 +2119,7 @@ namespace fcl {
bool inside = true;
for (int i = 0; i < 3; ++i) {
bool iinside = false;
FCL_REAL dist = Rb.col(i).dot(d), side_2 = b.side(i) / 2;
FCL_REAL dist = Rb.col(i).dot(d), side_2 = b.halfSide(i);
if (dist < -side_2) dist = -side_2; // outside
else if (dist > side_2) dist = side_2; // outside
else iinside = true; // inside
......
......@@ -76,10 +76,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_BOX:
{
const Box* box = static_cast<const Box*>(shape);
Vec3f res((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
(dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
(dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
return res;
return (dir.array() > 0).select(box->halfSide, -box->halfSide);
}
break;
case GEOM_SPHERE:
......
......@@ -50,16 +50,16 @@ namespace details
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf)
{
std::vector<Vec3f> result(8);
FCL_REAL a = box.side[0] / 2;
FCL_REAL b = box.side[1] / 2;
FCL_REAL c = box.side[2] / 2;
result[0] = tf.transform(Vec3f(a, b, c));
result[1] = tf.transform(Vec3f(a, b, -c));
result[2] = tf.transform(Vec3f(a, -b, c));
result[3] = tf.transform(Vec3f(a, -b, -c));
result[4] = tf.transform(Vec3f(-a, b, c));
result[5] = tf.transform(Vec3f(-a, b, -c));
result[6] = tf.transform(Vec3f(-a, -b, c));
FCL_REAL a = box.halfSide[0];
FCL_REAL b = box.halfSide[1];
FCL_REAL c = box.halfSide[2];
result[0] = tf.transform(Vec3f( a, b, c));
result[1] = tf.transform(Vec3f( a, b, -c));
result[2] = tf.transform(Vec3f( a, -b, c));
result[3] = tf.transform(Vec3f( a, -b, -c));
result[4] = tf.transform(Vec3f(-a, b, c));
result[5] = tf.transform(Vec3f(-a, b, -c));
result[6] = tf.transform(Vec3f(-a, -b, c));
result[7] = tf.transform(Vec3f(-a, -b, -c));
return result;
......@@ -255,7 +255,7 @@ void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
Vec3f v_delta (0.5 * R.cwiseAbs() * s.side);
Vec3f v_delta (R.cwiseAbs() * s.halfSide);
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
......@@ -405,9 +405,9 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
bv.To.noalias() = T;
bv.axes.noalias() = R;
bv.extent = s.side * (FCL_REAL)0.5;
bv.To = T;
bv.axes = R;
bv.extent = s.halfSide;
}
template<>
......
......@@ -72,18 +72,18 @@ void testBVHModelPointCloud()
}
Box box;
double a = box.side[0];
double b = box.side[1];
double c = box.side[2];
double a = box.halfSide[0];
double b = box.halfSide[1];
double c = box.halfSide[2];
std::vector<Vec3f> points(8);
points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
points[0] << a, -b, c;
points[1] << a, b, c;
points[2] << -a, b, c;
points[3] << -a, -b, c;
points[4] << a, -b, -c;
points[5] << a, b, -c;
points[6] << -a, b, -c;
points[7] << -a, -b, -c;
int result;
......@@ -113,19 +113,19 @@ void testBVHModelTriangles()
Box box(1,1,1);
AABB aabb (Vec3f(-1,0,-1), Vec3f(1,1,1));
double a = box.side[0];
double b = box.side[1];
double c = box.side[2];
double a = box.halfSide[0];
double b = box.halfSide[1];
double c = box.halfSide[2];
std::vector<Vec3f> points(8);
std::vector<Triangle> tri_indices(12);
points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
points[0] << a, -b, c;
points[1] << a, b, c;
points[2] << -a, b, c;
points[3] << -a, -b, c;
points[4] << a, -b, -c;
points[5] << a, b, -c;
points[6] << -a, b, -c;
points[7] << -a, -b, -c;
tri_indices[0].set(0, 4, 1);
tri_indices[1].set(1, 4, 5);
......@@ -202,19 +202,19 @@ void testBVHModelSubModel()
boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
Box box;
double a = box.side[0];
double b = box.side[1];
double c = box.side[2];
double a = box.halfSide[0];
double b = box.halfSide[1];
double c = box.halfSide[2];
std::vector<Vec3f> points(8);
std::vector<Triangle> tri_indices(12);
points[0] << 0.5 * a, -0.5 * b, 0.5 * c;
points[1] << 0.5 * a, 0.5 * b, 0.5 * c;
points[2] << -0.5 * a, 0.5 * b, 0.5 * c;
points[3] << -0.5 * a, -0.5 * b, 0.5 * c;
points[4] << 0.5 * a, -0.5 * b, -0.5 * c;
points[5] << 0.5 * a, 0.5 * b, -0.5 * c;
points[6] << -0.5 * a, 0.5 * b, -0.5 * c;
points[7] << -0.5 * a, -0.5 * b, -0.5 * c;
points[0] << a, -b, c;
points[1] << a, b, c;
points[2] << -a, b, c;
points[3] << -a, -b, c;
points[4] << a, -b, -c;
points[5] << a, b, -c;
points[6] << -a, b, -c;
points[7] << -a, -b, -c;
tri_indices[0].set(0, 4, 1);
tri_indices[1].set(1, 4, 5);
......
......@@ -296,9 +296,7 @@ BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
(p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1]
<= s1.radius));
Vec3f p1Loc (inverse (tf2).transform (p1));
bool p1_in_box ((fabs (p1Loc [0]) <= .5 * s2.side [0]) &&
(fabs (p1Loc [1]) <= .5 * s2.side [1]) &&
(fabs (p1Loc [2]) <= .5 * s2.side [2]));
bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all();
std::cout << "p2 in cylinder = (" << p2Loc.transpose () << ")" << std::endl;
std::cout << "p1 in box = (" << p1Loc.transpose () << ")" << std::endl;
......@@ -315,9 +313,7 @@ BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
(p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1]
<= s1.radius);
p1Loc = inverse (tf2).transform (p1);
p1_in_box = (fabs (p1Loc [0]) <= .5 * s2.side [0]) &&
(fabs (p1Loc [1]) <= .5 * s2.side [1]) &&
(fabs (p1Loc [2]) <= .5 * s2.side [2]);
p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all();
std::cout << "p2 in cylinder = (" << p2Loc.transpose () << ")" << std::endl;
std::cout << "p1 in box = (" << p1.transpose () << ")" << std::endl;
......@@ -440,9 +436,7 @@ void testBoxBoxContactPoints(const Matrix3f& R)
for (int i = 0; i < 8; ++i)
{
vertices[i][0] *= 0.5 * s2.side[0];
vertices[i][1] *= 0.5 * s2.side[1];
vertices[i][2] *= 0.5 * s2.side[2];
vertices[i].array() *= s2.halfSide.array();
}
Transform3f tf1 = Transform3f(Vec3f(0, 0, -50));
......
......@@ -114,8 +114,8 @@ BOOST_AUTO_TEST_CASE(obb_overlap)
// ShapeShapeDistance
gettimeofday (&t0, NULL);
for (std::size_t i=0; i<nbSamples; ++i) {
box1.side = 2*sample [i].extent1;
box2.side = 2*sample [i].extent2;
box1.halfSide = sample [i].extent1;
box2.halfSide = sample [i].extent2;
tf2.setTransform (sample [i].R, sample [i].T);
resultDistance [i].distance =
hpp::fcl::ShapeShapeDistance<hpp::fcl::Box, hpp::fcl::Box, hpp::fcl::GJKSolver_indep>
......
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