Skip to content

GitLab

Commit 7c60ab49 authored by Lucile Remigy's avatar Lucile Remigy
Browse files

changement nom de variable pour rendre le code plus claire

parent 3211b8ff
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 7c60ab49
......@@ -132,7 +132,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BVH/BVH_model.h
include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h
......
include/hpp/fcl/BV/BV.h
View file @ 7c60ab49
......@@ -125,7 +125,7 @@ class Converter<RSS, OBB>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent.noalias() = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.extent.noalias() = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
......@@ -168,9 +168,9 @@ public:
bv2.Tr = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.extent[2];
bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
bv2.radius = bv1.extent[2];
bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
}
};
......@@ -183,9 +183,9 @@ public:
bv2.Tr.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.r;
bv2.l[0] = bv1.l[0];
bv2.l[1] = bv1.l[1];
bv2.radius = bv1.radius;
bv2.length[0] = bv1.length[0];
bv2.length[1] = bv1.length[1];
}
};
......@@ -232,9 +232,9 @@ public:
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.r = extent[id[2]];
bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
bv2.radius = extent[id[2]];
bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
......
include/hpp/fcl/BV/RSS.h
View file @ 7c60ab49
......@@ -60,10 +60,10 @@ public:
Vec3f Tr;
/// @brief Side lengths of rectangle
FCL_REAL l[2];
FCL_REAL length[2];
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
FCL_REAL radius;
/// @brief Check whether the RSS contains a point
......@@ -101,7 +101,7 @@ public:
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline FCL_REAL size() const
{
return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + 2 * radius);
}
/// @brief The RSS center
......@@ -113,25 +113,25 @@ public:
/// @brief Width of the RSS
inline FCL_REAL width() const
{
return l[0] + 2 * r;
return length[0] + 2 * radius;
}
/// @brief Height of the RSS
inline FCL_REAL height() const
{
return l[1] + 2 * r;
return length[1] + 2 * radius;
}
/// @brief Depth of the RSS
inline FCL_REAL depth() const
{
return 2 * r;
return 2 * radius;
}
/// @brief Volume of the RSS
inline FCL_REAL volume() const
{
return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi<FCL_REAL>() * r * r * r);
return (length[0] * length[1] * 2 * radius + 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius);
}
/// @brief Check collision between two RSS and return the overlap part.
......
src/BV/RSS.cpp
View file @ 7c60ab49
......@@ -844,8 +844,8 @@ bool RSS::overlap(const RSS& other) const
/// Now compute R1'R2
Matrix3f R (axes.transpose() * other.axes);
FCL_REAL dist = rectDistance(R, T, l, other.l);
return (dist <= (r + other.r));
FCL_REAL dist = rectDistance(R, T, length, other.length);
return (dist <= (radius + other.radius));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
......@@ -859,8 +859,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l);
return (dist <= (b1.r + b2.r));
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length);
return (dist <= (b1.radius + b2.radius));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
......@@ -876,7 +876,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius;
if (dist <= 0) return true;
sqrDistLowerBound = dist * dist;
return false;
......@@ -893,28 +893,28 @@ bool RSS::contain(const Vec3f& p) const
Vec3f proj(proj0, proj1, proj2);
/// projection is within the rectangle
if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0))
{
return (abs_proj2 < r);
return (abs_proj2 < radius);
}
else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1])))
{
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(proj0, y, 0);
return ((proj - v).squaredNorm() < r * r);
return ((proj - v).squaredNorm() < radius * radius);
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0])))
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
Vec3f v(x, proj1, 0);
return ((proj - v).squaredNorm() < r * r);
return ((proj - v).squaredNorm() < radius * radius);
}
else
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(x, y, 0);
return ((proj - v).squaredNorm() < r * r);
return ((proj - v).squaredNorm() < radius * radius);
}
}
......@@ -928,99 +928,99 @@ RSS& RSS::operator += (const Vec3f& p)
Vec3f proj(proj0, proj1, proj2);
// projection is within the rectangle
if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0))
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
; // do nothing
else
{
r = 0.5 * (r + abs_proj2); // enlarge the r
radius = 0.5 * (radius + abs_proj2); // enlarge the r
// change RSS origin position
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);
else
Tr[2] -= 0.5 * (abs_proj2 - r);
Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1])))
{
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(proj0, y, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
if(new_r_sqr < radius * radius)
; // do nothing
else
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
{
FCL_REAL delta_y = - std::sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
l[1] += delta_y;
FCL_REAL delta_y = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y);
length[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
}
else
{
FCL_REAL delta_y = fabs(proj1 - y);
l[1] += delta_y;
length[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);
else
Tr[2] -= 0.5 * (abs_proj2 - r);
Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0])))
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
Vec3f v(x, proj1, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
if(new_r_sqr < radius * radius)
; // do nothing
else
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
{
FCL_REAL delta_x = - std::sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
l[0] += delta_x;
FCL_REAL delta_x = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x);
length[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
}
else
{
FCL_REAL delta_x = fabs(proj0 - x);
l[0] += delta_x;
length[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);
else
Tr[2] -= 0.5 * (abs_proj2 - r);
Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
else
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(x, y, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
if(new_r_sqr < radius * radius)
; // do nothing
else
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
{
FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2);
FCL_REAL delta_diag = - std::sqrt(r * r - proj2 * proj2) + diag;
FCL_REAL delta_diag = - std::sqrt(radius * radius - proj2 * proj2) + diag;
FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
l[0] += delta_x;
l[1] += delta_y;
length[0] += delta_x;
length[1] += delta_y;
if(proj0 < 0 && proj1 < 0)
{
......@@ -1033,8 +1033,8 @@ RSS& RSS::operator += (const Vec3f& p)
FCL_REAL delta_x = fabs(proj0 - x);
FCL_REAL delta_y = fabs(proj1 - y);
l[0] += delta_x;
l[1] += delta_y;
length[0] += delta_x;
length[1] += delta_y;
if(proj0 < 0 && proj1 < 0)
{
......@@ -1043,9 +1043,9 @@ RSS& RSS::operator += (const Vec3f& p)
}
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);
else
Tr[2] -= 0.5 * (abs_proj2 - r);
Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
......@@ -1058,12 +1058,12 @@ RSS RSS::operator + (const RSS& other) const
RSS bv;
Vec3f v[16];
Vec3f d0_pos (other.axes.col(0) * (other.l[0] + other.r));
Vec3f d1_pos (other.axes.col(1) * (other.l[1] + other.r));
Vec3f d0_neg (other.axes.col(0) * (-other.r));
Vec3f d1_neg (other.axes.col(1) * (-other.r));
Vec3f d2_pos (other.axes.col(2) * other.r);
Vec3f d2_neg (other.axes.col(2) * (-other.r));
Vec3f d0_pos (other.axes.col(0) * (other.length[0] + other.radius));
Vec3f d1_pos (other.axes.col(1) * (other.length[1] + other.radius));
Vec3f d0_neg (other.axes.col(0) * (-other.radius));
Vec3f d1_neg (other.axes.col(1) * (-other.radius));
Vec3f d2_pos (other.axes.col(2) * other.radius);
Vec3f d2_neg (other.axes.col(2) * (-other.radius));
v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
......@@ -1074,12 +1074,12 @@ RSS RSS::operator + (const RSS& other) const
v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos;
v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg;
d0_pos.noalias() = axes.col(0) * (l[0] + r);
d1_pos.noalias() = axes.col(1) * (l[1] + r);
d0_neg.noalias() = axes.col(0) * (-r);
d1_neg.noalias() = axes.col(1) * (-r);
d2_pos.noalias() = axes.col(2) * r;
d2_neg.noalias() = axes.col(2) * (-r);
d0_pos.noalias() = axes.col(0) * (length[0] + radius);
d1_pos.noalias() = axes.col(1) * (length[1] + radius);
d0_neg.noalias() = axes.col(0) * (-radius);
d1_neg.noalias() = axes.col(1) * (-radius);
d2_pos.noalias() = axes.col(2) * radius;
d2_neg.noalias() = axes.col(2) * (-radius);
v[ 8].noalias() = Tr + d0_pos + d1_pos + d2_pos;
v[ 9].noalias() = Tr + d0_pos + d1_pos + d2_neg;
......@@ -1113,7 +1113,7 @@ RSS RSS::operator + (const RSS& other) const
E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.l, bv.r);
getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.length, bv.radius);
return bv;
}
......@@ -1126,8 +1126,8 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
Matrix3f R (axes.transpose() * other.axes);
Vec3f T (axes.transpose() * (other.Tr - Tr));
FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
dist -= (r + other.r);
FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q);
dist -= (radius + other.radius);
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
......@@ -1138,8 +1138,8 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
Vec3f T(b1.axes.transpose() * Ttemp);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q);
dist -= (b1.r + b2.r);
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q);
dist -= (b1.radius + b2.radius);
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
......
src/BVH/BV_fitter.cpp
View file @ 7c60ab49
......@@ -150,9 +150,9 @@ void fit1(Vec3f* ps, RSS& bv)
{
bv.Tr.noalias() = ps[0];
bv.axes.setIdentity();
bv.l[0] = 0;
bv.l[1] = 0;
bv.r = 0;
bv.length[0] = 0;
bv.length[1] = 0;
bv.radius = 0;
}
void fit2(Vec3f* ps, RSS& bv)
......@@ -164,11 +164,11 @@ void fit2(Vec3f* ps, RSS& bv)
bv.axes.col(0) /= len_p1p2;
generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
bv.l[0] = len_p1p2;
bv.l[1] = 0;
bv.length[0] = len_p1p2;
bv.length[1] = 0;
bv.Tr = p2;
bv.r = 0;
bv.radius = 0;
}
void fit3(Vec3f* ps, RSS& bv)
......@@ -193,7 +193,7 @@ void fit3(Vec3f* ps, RSS& bv)
bv.axes.col(0).noalias() = e[imax].normalized();
bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.l, bv.r);
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.length, bv.radius);
}
void fit6(Vec3f* ps, RSS& bv)
......@@ -215,7 +215,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
axisFromEigen(E, s, bv.axes);
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.l, bv.r);
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.length, bv.radius);
}
}
......@@ -542,9 +542,9 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r);
bv.rss.Tr = origin;
bv.rss.l[0] = l[0];
bv.rss.l[1] = l[1];
bv.rss.r = r;
bv.rss.length[0] = l[0];
bv.rss.length[1] = l[1];
bv.rss.radius = r;
return bv;
}
......@@ -568,9 +568,9 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r);
bv.Tr = origin;
bv.l[0] = l[0];
bv.l[1] = l[1];
bv.r = r;
bv.length[0] = l[0];
bv.length[1] = l[1];
bv.radius = r;
return bv;
......
src/shape/geometric_shapes_utility.cpp
View file @ 7c60ab49
......@@ -482,7 +482,7 @@ void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv)
/// Half space can only have very rough RSS
bv.axes.setIdentity();
bv.Tr.setZero();
bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
bv.length[0] = bv.length[1] = bv.radius = std::numeric_limits<FCL_REAL>::max();
}
template<>
......@@ -716,10 +716,10 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
bv.l[0] = std::numeric_limits<FCL_REAL>::max();
bv.l[1] = std::numeric_limits<FCL_REAL>::max();
bv.length[0] = std::numeric_limits<FCL_REAL>::max();
bv.length[1] = std::numeric_limits<FCL_REAL>::max();
bv.r = 0;
bv.radius = 0;
Vec3f p = s.n * s.d;
bv.Tr = tf.transform(p);
......
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