From 3351029d1b29fa4fe9eb8b8b72772a87fb568cfc Mon Sep 17 00:00:00 2001
From: LucileRemigy <45563869+LucileRemigy@users.noreply.github.com>
Date: Thu, 14 Nov 2019 11:33:21 +0100
Subject: [PATCH] Delete lz for Cone, Cylinder and Capsule, now use halfLength
(#98)
---
include/hpp/fcl/shape/geometric_shapes.h | 47 +++++++++---------------
python/collision-geometries.cc | 6 +--
src/distance_capsule_capsule.cpp | 4 +-
src/narrowphase/details.h | 44 +++++++++++-----------
src/narrowphase/gjk.cpp | 8 ++--
src/shape/geometric_shapes_utility.cpp | 26 ++++++-------
test/geometric_shapes.cpp | 4 +-
test/python_unit/geometric_shapes.py | 14 +++----
8 files changed, 71 insertions(+), 82 deletions(-)
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 68c5cf07..d991f4e0 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -151,20 +151,16 @@ public:
class Capsule : public ShapeBase
{
public:
- Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+ Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
{
- lz = lz_;
- HalfLength = lz_/2;
+ halfLength = lz_/2;
}
/// @brief Radius of capsule
FCL_REAL radius;
- /// @brief Length along z axis
- FCL_REAL lz;
-
/// @brief Half Length along z axis
- FCL_REAL HalfLength;
+ FCL_REAL halfLength;
/// @brief Compute AABB
void computeLocalAABB();
@@ -174,15 +170,15 @@ public:
FCL_REAL computeVolume() const
{
- return boost::math::constants::pi<FCL_REAL>() * radius * radius *(lz + radius * 4/3.0);
+ return boost::math::constants::pi<FCL_REAL>() * radius * radius *((halfLength * 2) + radius * 4/3.0);
}
Matrix3f computeMomentofInertia() const
{
- FCL_REAL v_cyl = radius * radius * lz * boost::math::constants::pi<FCL_REAL>();
+ FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
- FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz;
+ FCL_REAL ix = v_cyl * halfLength * halfLength / 3. + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + v_sph * halfLength * halfLength;
FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
return (Matrix3f() << ix, 0, 0,
@@ -196,20 +192,16 @@ public:
class Cone : public ShapeBase
{
public:
- Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+ Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
{
- lz = lz_;
- HalfLength = lz_/2;
+ halfLength = lz_/2;
}
/// @brief Radius of the cone
FCL_REAL radius;
- /// @brief Length along z axis
- FCL_REAL lz;
-
/// @brief Half Length along z axis
- FCL_REAL HalfLength;
+ FCL_REAL halfLength;
/// @brief Compute AABB
void computeLocalAABB();
@@ -219,13 +211,14 @@ public:
FCL_REAL computeVolume() const
{
- return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz / 3;
+ return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
}
+ /// \todo verify this formula as it seems different from https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
- FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20);
+ FCL_REAL ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
FCL_REAL iz = 0.3 * V * radius * radius;
return (Matrix3f() << ix, 0, 0,
@@ -235,7 +228,7 @@ public:
Vec3f computeCOM() const
{
- return Vec3f(0, 0, -0.25 * lz);
+ return Vec3f(0, 0, -0.5 * halfLength);
}
};
@@ -243,20 +236,16 @@ public:
class Cylinder : public ShapeBase
{
public:
- Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+ Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
{
- lz = lz_;
- HalfLength = lz_/2;
+ halfLength = lz_/2;
}
/// @brief Radius of the cylinder
FCL_REAL radius;
- /// @brief Length along z axis
- FCL_REAL lz;
-
/// @brief Half Length along z axis
- FCL_REAL HalfLength;
+ FCL_REAL halfLength;
/// @brief Compute AABB
void computeLocalAABB();
@@ -266,13 +255,13 @@ public:
FCL_REAL computeVolume() const
{
- return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz;
+ return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2);
}
Matrix3f computeMomentofInertia() const
{
FCL_REAL V = computeVolume();
- FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12;
+ FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
FCL_REAL iz = V * radius * radius / 2;
return (Matrix3f() << ix, 0, 0,
0, ix, 0,
diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc
index 7d3b63f9..6f2744b6 100644
--- a/python/collision-geometries.cc
+++ b/python/collision-geometries.cc
@@ -122,13 +122,13 @@ void exposeShapes ()
class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
("Capsule", init<FCL_REAL, FCL_REAL>())
.def_readwrite ("radius", &Capsule::radius)
- .def_readwrite ("lz", &Capsule::lz)
+ .def_readwrite ("halfLength", &Capsule::halfLength)
;
class_ <Cone, bases<ShapeBase>, shared_ptr<Cone> >
("Cone", init<FCL_REAL, FCL_REAL>())
.def_readwrite ("radius", &Cone::radius)
- .def_readwrite ("lz", &Cone::lz)
+ .def_readwrite ("halfLength", &Cone::halfLength)
;
class_ <ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase >, noncopyable>
@@ -148,7 +148,7 @@ void exposeShapes ()
class_ <Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >
("Cylinder", init<FCL_REAL, FCL_REAL>())
.def_readwrite ("radius", &Cylinder::radius)
- .def_readwrite ("lz", &Cylinder::lz)
+ .def_readwrite ("halfLength", &Cylinder::halfLength)
;
class_ <Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 2762830f..029c2368 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -41,8 +41,8 @@ namespace fcl {
// We assume that capsules are oriented along z-axis.
Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2);
Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2);
- FCL_REAL halfLength1 = 0.5*c1->lz;
- FCL_REAL halfLength2 = 0.5*c2->lz;
+ FCL_REAL halfLength1 = c1->halfLength;
+ FCL_REAL halfLength2 = c2->halfLength;
Vec3f diff = center1 - center2;
FCL_REAL a01 = -direction1.dot (direction2);
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index ad853e65..58036184 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -73,8 +73,8 @@ namespace fcl {
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
{
- Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function
- Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+ Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); // from distance function
+ Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
Vec3f s_c = tf1.getTranslation ();
Vec3f segment_point;
@@ -108,8 +108,8 @@ namespace fcl {
const Capsule& s2, const Transform3f& tf2,
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
{
- Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
- Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+ Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength)));
+ Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
Vec3f s_c = tf1.getTranslation ();
Vec3f segment_point;
@@ -143,7 +143,7 @@ namespace fcl {
static const FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ()));
FCL_REAL r1 (s1.radius);
FCL_REAL r2 (s2.radius);
- FCL_REAL lz2 (.5*s2.lz);
+ FCL_REAL lz2 (s2.halfLength);
// boundaries of the cylinder axis
Vec3f A (tf2.transform (Vec3f (0, 0, -lz2)));
Vec3f B (tf2.transform (Vec3f (0, 0, lz2)));
@@ -151,7 +151,7 @@ namespace fcl {
Vec3f S (tf1.getTranslation ());
// axis of the cylinder
Vec3f u (tf2.getRotation ().col (2));
- assert ((B - A - s2.lz * u).norm () < eps);
+ assert ((B - A - (s2.halfLength * 2) * u).norm () < eps);
Vec3f AS (S - A);
// abscissa of S on cylinder axis with A as the origin
FCL_REAL s (u.dot (AS));
@@ -186,7 +186,7 @@ namespace fcl {
dist = -r1;
}
}
- } else if (s <= s2.lz) {
+ } else if (s <= (s2.halfLength * 2)) {
// 0 < s <= s2.lz
normal = -v;
dist = dPS - r1 - r2;
@@ -200,7 +200,7 @@ namespace fcl {
// lz < s
if (dPS <= r2) {
// closest point on cylinder is on cylinder disc basis
- dist = s - s2.lz - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
+ dist = s - (s2.halfLength * 2) - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
} else {
// closest point on cylinder is on cylinder circle basis
p2 = B + r2 * v;
@@ -1605,7 +1605,7 @@ namespace fcl {
int sign = (cosa > 0) ? -1 : 1;
// closest capsule vertex to halfspace if no collision,
// or deeper inside halspace if collision
- Vec3f p = T + dir_z * (s1.lz * 0.5 * sign);
+ Vec3f p = T + dir_z * (s1.halfLength * sign);
FCL_REAL signed_dist = new_s2.signedDistance(p);
distance = signed_dist - s1.radius;
@@ -1666,7 +1666,7 @@ namespace fcl {
int sign = (cosa > 0) ? -1 : 1;
// deepest point
- Vec3f p = T + dir_z * (s1.lz * 0.5 * sign) + C;
+ Vec3f p = T + dir_z * (s1.halfLength * sign) + C;
distance = new_s2.signedDistance(p);
if(distance > 0) {
// TODO: compute closest points
@@ -1708,7 +1708,7 @@ namespace fcl {
else
{
normal = -new_s2.n;
- p1 = p2 = T - dir_z * (s1.lz * 0.5) -
+ p1 = p2 = T - dir_z * (s1.halfLength) -
new_s2.n * (0.5 * distance + s1.radius);
return true;
}
@@ -1726,8 +1726,8 @@ namespace fcl {
C *= s;
}
- Vec3f a1 = T + dir_z * (0.5 * s1.lz);
- Vec3f a2 = T - dir_z * (0.5 * s1.lz) + C;
+ Vec3f a1 = T + dir_z * (s1.halfLength);
+ Vec3f a2 = T - dir_z * (s1.halfLength) + C;
FCL_REAL d1 = new_s2.signedDistance(a1);
FCL_REAL d2 = new_s2.signedDistance(a2);
@@ -2153,8 +2153,8 @@ namespace fcl {
Vec3f dir_z = R1.col(2);
// ends of capsule inner segment
- Vec3f a1 = T1 + dir_z * (0.5 * s1.lz);
- Vec3f a2 = T1 - dir_z * (0.5 * s1.lz);
+ Vec3f a1 = T1 + dir_z * s1.halfLength;
+ Vec3f a2 = T1 - dir_z * s1.halfLength;
FCL_REAL d1 = new_s2.signedDistance(a1);
FCL_REAL d2 = new_s2.signedDistance(a2);
@@ -2278,8 +2278,8 @@ namespace fcl {
C *= s;
}
- Vec3f a1 = T + dir_z * (0.5 * s1.lz);
- Vec3f a2 = T - dir_z * (0.5 * s1.lz);
+ Vec3f a1 = T + dir_z * (s1.halfLength);
+ Vec3f a2 = T - dir_z * (s1.halfLength);
Vec3f c1, c2;
if(cosa > 0)
@@ -2346,8 +2346,8 @@ namespace fcl {
else
{
if (d < 0) normal = new_s2.n; else normal = -new_s2.n;
- p1 = p2 = T - dir_z * (0.5 * s1.lz) +
- dir_z * (-0.5 * distance / s1.radius * s1.lz) - new_s2.n * d;
+ p1 = p2 = T - dir_z * (s1.halfLength) +
+ dir_z * (- distance / s1.radius * s1.halfLength) - new_s2.n * d;
return true;
}
}
@@ -2365,9 +2365,9 @@ namespace fcl {
}
Vec3f c[3];
- c[0] = T + dir_z * (0.5 * s1.lz);
- c[1] = T - dir_z * (0.5 * s1.lz) + C;
- c[2] = T - dir_z * (0.5 * s1.lz) - C;
+ c[0] = T + dir_z * (s1.halfLength);
+ c[1] = T - dir_z * (s1.halfLength) + C;
+ c[2] = T - dir_z * (s1.halfLength) - C;
FCL_REAL d[3];
d[0] = new_s2.signedDistance(c[0]);
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 564233df..9f0b94db 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -119,8 +119,8 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& suppo
inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
{
support = capsule->radius * dir;
- if (dir[2] > 0) support[2] += capsule->lz / 2;
- else support[2] -= capsule->lz / 2;
+ if (dir[2] > 0) support[2] += capsule->halfLength;
+ else support[2] -= capsule->halfLength;
}
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
@@ -137,7 +137,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
FCL_REAL len = zdist + dir[2] * dir[2];
zdist = std::sqrt(zdist);
len = std::sqrt(len);
- FCL_REAL half_h = cone->lz * 0.5;
+ FCL_REAL half_h = cone->halfLength;
FCL_REAL radius = cone->radius;
FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
@@ -156,7 +156,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
{
static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
- FCL_REAL half_h = cylinder->lz * 0.5;
+ FCL_REAL half_h = cylinder->halfLength;
if (dir [2] > eps) support[2] = half_h;
else if (dir [2] < -eps) support[2] = -half_h;
else support[2] = 0;
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index c70049c9..c0aba492 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -96,7 +96,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t
std::vector<Vec3f> result(36);
const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
- FCL_REAL hl = capsule.lz * 0.5;
+ FCL_REAL hl = capsule.halfLength;
FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
FCL_REAL a = edge_size;
FCL_REAL b = m * edge_size;
@@ -153,7 +153,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
{
std::vector<Vec3f> result(7);
- FCL_REAL hl = cone.lz * 0.5;
+ FCL_REAL hl = cone.halfLength;
FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
FCL_REAL a = 0.5 * r2;
FCL_REAL b = cone.radius;
@@ -174,7 +174,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f&
{
std::vector<Vec3f> result(12);
- FCL_REAL hl = cylinder.lz * 0.5;
+ FCL_REAL hl = cylinder.halfLength;
FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
FCL_REAL a = 0.5 * r2;
FCL_REAL b = cylinder.radius;
@@ -277,7 +277,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- Vec3f v_delta(0.5 * (R.col(2)*s.lz).cwiseAbs() + Vec3f::Constant(s.radius));
+ Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
@@ -288,9 +288,9 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
- FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
- FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
+ FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength);
+ FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength);
+ FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);
Vec3f v_delta(x_range, y_range, z_range);
bv.max_ = T + v_delta;
@@ -303,9 +303,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& b
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
- FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
- FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
+ FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength);
+ FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength);
+ FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);
Vec3f v_delta(x_range, y_range, z_range);
bv.max_ = T + v_delta;
@@ -429,7 +429,7 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
bv.To.noalias() = T;
bv.axes.noalias() = R;
- bv.extent << s.radius, s.radius, s.lz / 2 + s.radius;
+ bv.extent << s.radius, s.radius, s.halfLength + s.radius;
}
template<>
@@ -440,7 +440,7 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
bv.To.noalias() = T;
bv.axes.noalias() = R;
- bv.extent << s.radius, s.radius, s.lz / 2;
+ bv.extent << s.radius, s.radius, s.halfLength;
}
template<>
@@ -451,7 +451,7 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
bv.To.noalias() = T;
bv.axes.noalias() = R;
- bv.extent << s.radius, s.radius, s.lz / 2;
+ bv.extent << s.radius, s.radius, s.halfLength;
}
template<>
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index 38462d91..08b22fec 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -211,7 +211,7 @@ BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
// If objects are not colliding, p2 should be outside the cylinder and
// p1 should be outside the box
Vec3f p2Loc (tf1.inverse().transform (p2));
- bool p2_in_cylinder ((fabs (p2Loc [2]) <= .5*s1.lz) &&
+ bool p2_in_cylinder ((fabs (p2Loc [2]) <= s1.halfLength) &&
(p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1]
<= s1.radius));
Vec3f p1Loc (tf2.inverse().transform (p1));
@@ -228,7 +228,7 @@ BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
// p1 should be outside the box
p2Loc = tf1.inverse().transform (p2);
- p2_in_cylinder = (fabs (p2Loc [2]) <= .5*s1.lz) &&
+ p2_in_cylinder = (fabs (p2Loc [2]) <= s1.halfLength) &&
(p2Loc [0] * p2Loc [0] + p2Loc [1] * p2Loc [1]
<= s1.radius);
p1Loc = tf2.inverse().transform (p1);
diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py
index 466d92e3..6e9109e0 100644
--- a/test/python_unit/geometric_shapes.py
+++ b/test/python_unit/geometric_shapes.py
@@ -12,11 +12,11 @@ class TestGeometricShapes(unittest.TestCase):
self.assertIsInstance(capsule, hppfcl.CollisionGeometry)
self.assertEqual(capsule.getNodeType(), hppfcl.NODE_TYPE.GEOM_CAPSULE)
self.assertEqual(capsule.radius,1.)
- self.assertEqual(capsule.lz,2.)
+ self.assertEqual(capsule.halfLength,1.)
capsule.radius = 3.
- capsule.lz = 4.
+ capsule.halfLength = 4.
self.assertEqual(capsule.radius,3.)
- self.assertEqual(capsule.lz,4.)
+ self.assertEqual(capsule.halfLength,4.)
def test_box1(self):
box = hppfcl.Box(np.matrix([1.,2.,3.]).T)
@@ -61,7 +61,7 @@ class TestGeometricShapes(unittest.TestCase):
self.assertIsInstance(cylinder, hppfcl.CollisionGeometry)
self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER)
self.assertEqual(cylinder.radius,1.)
- self.assertEqual(cylinder.lz,2.)
+ self.assertEqual(cylinder.halfLength,1.)
def test_cone(self):
cone = hppfcl.Cone(1.,2.)
@@ -70,11 +70,11 @@ class TestGeometricShapes(unittest.TestCase):
self.assertIsInstance(cone, hppfcl.CollisionGeometry)
self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE)
self.assertEqual(cone.radius,1.)
- self.assertEqual(cone.lz,2.)
+ self.assertEqual(cone.halfLength,1.)
cone.radius = 3.
- cone.lz = 4.
+ cone.halfLength = 4.
self.assertEqual(cone.radius,3.)
- self.assertEqual(cone.lz,4.)
+ self.assertEqual(cone.halfLength,4.)
if __name__ == '__main__':
unittest.main()
\ No newline at end of file
--
GitLab