Commit 3351029d authored by LucileRemigy's avatar LucileRemigy Committed by Joseph Mirabel
Browse files

Delete lz for Cone, Cylinder and Capsule, now use halfLength (#98)

parent 36604bee
...@@ -151,20 +151,16 @@ public: ...@@ -151,20 +151,16 @@ public:
class Capsule : public ShapeBase class Capsule : public ShapeBase
{ {
public: 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 /// @brief Radius of capsule
FCL_REAL radius; FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Half Length along z axis /// @brief Half Length along z axis
FCL_REAL HalfLength; FCL_REAL halfLength;
/// @brief Compute AABB /// @brief Compute AABB
void computeLocalAABB(); void computeLocalAABB();
...@@ -174,15 +170,15 @@ public: ...@@ -174,15 +170,15 @@ public:
FCL_REAL computeVolume() const 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 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 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; FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
return (Matrix3f() << ix, 0, 0, return (Matrix3f() << ix, 0, 0,
...@@ -196,20 +192,16 @@ public: ...@@ -196,20 +192,16 @@ public:
class Cone : public ShapeBase class Cone : public ShapeBase
{ {
public: 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 /// @brief Radius of the cone
FCL_REAL radius; FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Half Length along z axis /// @brief Half Length along z axis
FCL_REAL HalfLength; FCL_REAL halfLength;
/// @brief Compute AABB /// @brief Compute AABB
void computeLocalAABB(); void computeLocalAABB();
...@@ -219,13 +211,14 @@ public: ...@@ -219,13 +211,14 @@ public:
FCL_REAL computeVolume() const 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 Matrix3f computeMomentofInertia() const
{ {
FCL_REAL V = computeVolume(); 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; FCL_REAL iz = 0.3 * V * radius * radius;
return (Matrix3f() << ix, 0, 0, return (Matrix3f() << ix, 0, 0,
...@@ -235,7 +228,7 @@ public: ...@@ -235,7 +228,7 @@ public:
Vec3f computeCOM() const Vec3f computeCOM() const
{ {
return Vec3f(0, 0, -0.25 * lz); return Vec3f(0, 0, -0.5 * halfLength);
} }
}; };
...@@ -243,20 +236,16 @@ public: ...@@ -243,20 +236,16 @@ public:
class Cylinder : public ShapeBase class Cylinder : public ShapeBase
{ {
public: 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 /// @brief Radius of the cylinder
FCL_REAL radius; FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Half Length along z axis /// @brief Half Length along z axis
FCL_REAL HalfLength; FCL_REAL halfLength;
/// @brief Compute AABB /// @brief Compute AABB
void computeLocalAABB(); void computeLocalAABB();
...@@ -266,13 +255,13 @@ public: ...@@ -266,13 +255,13 @@ public:
FCL_REAL computeVolume() const 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 Matrix3f computeMomentofInertia() const
{ {
FCL_REAL V = computeVolume(); 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; FCL_REAL iz = V * radius * radius / 2;
return (Matrix3f() << ix, 0, 0, return (Matrix3f() << ix, 0, 0,
0, ix, 0, 0, ix, 0,
......
...@@ -122,13 +122,13 @@ void exposeShapes () ...@@ -122,13 +122,13 @@ void exposeShapes ()
class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> > class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
("Capsule", init<FCL_REAL, FCL_REAL>()) ("Capsule", init<FCL_REAL, FCL_REAL>())
.def_readwrite ("radius", &Capsule::radius) .def_readwrite ("radius", &Capsule::radius)
.def_readwrite ("lz", &Capsule::lz) .def_readwrite ("halfLength", &Capsule::halfLength)
; ;
class_ <Cone, bases<ShapeBase>, shared_ptr<Cone> > class_ <Cone, bases<ShapeBase>, shared_ptr<Cone> >
("Cone", init<FCL_REAL, FCL_REAL>()) ("Cone", init<FCL_REAL, FCL_REAL>())
.def_readwrite ("radius", &Cone::radius) .def_readwrite ("radius", &Cone::radius)
.def_readwrite ("lz", &Cone::lz) .def_readwrite ("halfLength", &Cone::halfLength)
; ;
class_ <ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase >, noncopyable> class_ <ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase >, noncopyable>
...@@ -148,7 +148,7 @@ void exposeShapes () ...@@ -148,7 +148,7 @@ void exposeShapes ()
class_ <Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> > class_ <Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >
("Cylinder", init<FCL_REAL, FCL_REAL>()) ("Cylinder", init<FCL_REAL, FCL_REAL>())
.def_readwrite ("radius", &Cylinder::radius) .def_readwrite ("radius", &Cylinder::radius)
.def_readwrite ("lz", &Cylinder::lz) .def_readwrite ("halfLength", &Cylinder::halfLength)
; ;
class_ <Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> > class_ <Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >
......
...@@ -41,8 +41,8 @@ namespace fcl { ...@@ -41,8 +41,8 @@ namespace fcl {
// We assume that capsules are oriented along z-axis. // We assume that capsules are oriented along z-axis.
Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2); Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2);
Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2); Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2);
FCL_REAL halfLength1 = 0.5*c1->lz; FCL_REAL halfLength1 = c1->halfLength;
FCL_REAL halfLength2 = 0.5*c2->lz; FCL_REAL halfLength2 = c2->halfLength;
Vec3f diff = center1 - center2; Vec3f diff = center1 - center2;
FCL_REAL a01 = -direction1.dot (direction2); FCL_REAL a01 = -direction1.dot (direction2);
......
...@@ -73,8 +73,8 @@ namespace fcl { ...@@ -73,8 +73,8 @@ namespace fcl {
const Capsule& s2, const Transform3f& tf2, const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_) Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
{ {
Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); // from distance function
Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz))); Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
Vec3f s_c = tf1.getTranslation (); Vec3f s_c = tf1.getTranslation ();
Vec3f segment_point; Vec3f segment_point;
...@@ -108,8 +108,8 @@ namespace fcl { ...@@ -108,8 +108,8 @@ namespace fcl {
const Capsule& s2, const Transform3f& tf2, const Capsule& s2, const Transform3f& tf2,
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
{ {
Vec3f pos1 (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., -0.5 * s2.lz))); Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
Vec3f s_c = tf1.getTranslation (); Vec3f s_c = tf1.getTranslation ();
Vec3f segment_point; Vec3f segment_point;
...@@ -143,7 +143,7 @@ namespace fcl { ...@@ -143,7 +143,7 @@ namespace fcl {
static const FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ())); static const FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ()));
FCL_REAL r1 (s1.radius); FCL_REAL r1 (s1.radius);
FCL_REAL r2 (s2.radius); FCL_REAL r2 (s2.radius);
FCL_REAL lz2 (.5*s2.lz); FCL_REAL lz2 (s2.halfLength);
// boundaries of the cylinder axis // boundaries of the cylinder axis
Vec3f A (tf2.transform (Vec3f (0, 0, -lz2))); Vec3f A (tf2.transform (Vec3f (0, 0, -lz2)));
Vec3f B (tf2.transform (Vec3f (0, 0, lz2))); Vec3f B (tf2.transform (Vec3f (0, 0, lz2)));
...@@ -151,7 +151,7 @@ namespace fcl { ...@@ -151,7 +151,7 @@ namespace fcl {
Vec3f S (tf1.getTranslation ()); Vec3f S (tf1.getTranslation ());
// axis of the cylinder // axis of the cylinder
Vec3f u (tf2.getRotation ().col (2)); 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); Vec3f AS (S - A);
// abscissa of S on cylinder axis with A as the origin // abscissa of S on cylinder axis with A as the origin
FCL_REAL s (u.dot (AS)); FCL_REAL s (u.dot (AS));
...@@ -186,7 +186,7 @@ namespace fcl { ...@@ -186,7 +186,7 @@ namespace fcl {
dist = -r1; dist = -r1;
} }
} }
} else if (s <= s2.lz) { } else if (s <= (s2.halfLength * 2)) {
// 0 < s <= s2.lz // 0 < s <= s2.lz
normal = -v; normal = -v;
dist = dPS - r1 - r2; dist = dPS - r1 - r2;
...@@ -200,7 +200,7 @@ namespace fcl { ...@@ -200,7 +200,7 @@ namespace fcl {
// lz < s // lz < s
if (dPS <= r2) { if (dPS <= r2) {
// closest point on cylinder is on cylinder disc basis // 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 { } else {
// closest point on cylinder is on cylinder circle basis // closest point on cylinder is on cylinder circle basis
p2 = B + r2 * v; p2 = B + r2 * v;
...@@ -1605,7 +1605,7 @@ namespace fcl { ...@@ -1605,7 +1605,7 @@ namespace fcl {
int sign = (cosa > 0) ? -1 : 1; int sign = (cosa > 0) ? -1 : 1;
// closest capsule vertex to halfspace if no collision, // closest capsule vertex to halfspace if no collision,
// or deeper inside halspace if 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); FCL_REAL signed_dist = new_s2.signedDistance(p);
distance = signed_dist - s1.radius; distance = signed_dist - s1.radius;
...@@ -1666,7 +1666,7 @@ namespace fcl { ...@@ -1666,7 +1666,7 @@ namespace fcl {
int sign = (cosa > 0) ? -1 : 1; int sign = (cosa > 0) ? -1 : 1;
// deepest point // 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); distance = new_s2.signedDistance(p);
if(distance > 0) { if(distance > 0) {
// TODO: compute closest points // TODO: compute closest points
...@@ -1708,7 +1708,7 @@ namespace fcl { ...@@ -1708,7 +1708,7 @@ namespace fcl {
else else
{ {
normal = -new_s2.n; 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); new_s2.n * (0.5 * distance + s1.radius);
return true; return true;
} }
...@@ -1726,8 +1726,8 @@ namespace fcl { ...@@ -1726,8 +1726,8 @@ namespace fcl {
C *= s; C *= s;
} }
Vec3f a1 = T + dir_z * (0.5 * s1.lz); Vec3f a1 = T + dir_z * (s1.halfLength);
Vec3f a2 = T - dir_z * (0.5 * s1.lz) + C; Vec3f a2 = T - dir_z * (s1.halfLength) + C;
FCL_REAL d1 = new_s2.signedDistance(a1); FCL_REAL d1 = new_s2.signedDistance(a1);
FCL_REAL d2 = new_s2.signedDistance(a2); FCL_REAL d2 = new_s2.signedDistance(a2);
...@@ -2153,8 +2153,8 @@ namespace fcl { ...@@ -2153,8 +2153,8 @@ namespace fcl {
Vec3f dir_z = R1.col(2); Vec3f dir_z = R1.col(2);
// ends of capsule inner segment // ends of capsule inner segment
Vec3f a1 = T1 + dir_z * (0.5 * s1.lz); Vec3f a1 = T1 + dir_z * s1.halfLength;
Vec3f a2 = T1 - dir_z * (0.5 * s1.lz); Vec3f a2 = T1 - dir_z * s1.halfLength;
FCL_REAL d1 = new_s2.signedDistance(a1); FCL_REAL d1 = new_s2.signedDistance(a1);
FCL_REAL d2 = new_s2.signedDistance(a2); FCL_REAL d2 = new_s2.signedDistance(a2);
...@@ -2278,8 +2278,8 @@ namespace fcl { ...@@ -2278,8 +2278,8 @@ namespace fcl {
C *= s; C *= s;
} }
Vec3f a1 = T + dir_z * (0.5 * s1.lz); Vec3f a1 = T + dir_z * (s1.halfLength);
Vec3f a2 = T - dir_z * (0.5 * s1.lz); Vec3f a2 = T - dir_z * (s1.halfLength);
Vec3f c1, c2; Vec3f c1, c2;
if(cosa > 0) if(cosa > 0)
...@@ -2346,8 +2346,8 @@ namespace fcl { ...@@ -2346,8 +2346,8 @@ namespace fcl {
else else
{ {
if (d < 0) normal = new_s2.n; else normal = -new_s2.n; if (d < 0) normal = new_s2.n; else normal = -new_s2.n;
p1 = p2 = T - dir_z * (0.5 * s1.lz) + p1 = p2 = T - dir_z * (s1.halfLength) +
dir_z * (-0.5 * distance / s1.radius * s1.lz) - new_s2.n * d; dir_z * (- distance / s1.radius * s1.halfLength) - new_s2.n * d;
return true; return true;
} }
} }
...@@ -2365,9 +2365,9 @@ namespace fcl { ...@@ -2365,9 +2365,9 @@ namespace fcl {
} }
Vec3f c[3]; Vec3f c[3];
c[0] = T + dir_z * (0.5 * s1.lz); c[0] = T + dir_z * (s1.halfLength);
c[1] = T - dir_z * (0.5 * s1.lz) + C; c[1] = T - dir_z * (s1.halfLength) + C;
c[2] = T - dir_z * (0.5 * s1.lz) - C; c[2] = T - dir_z * (s1.halfLength) - C;
FCL_REAL d[3]; FCL_REAL d[3];
d[0] = new_s2.signedDistance(c[0]); d[0] = new_s2.signedDistance(c[0]);
......
...@@ -119,8 +119,8 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& suppo ...@@ -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) inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
{ {
support = capsule->radius * dir; support = capsule->radius * dir;
if (dir[2] > 0) support[2] += capsule->lz / 2; if (dir[2] > 0) support[2] += capsule->halfLength;
else support[2] -= capsule->lz / 2; else support[2] -= capsule->halfLength;
} }
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support) void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
...@@ -137,7 +137,7 @@ 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]; FCL_REAL len = zdist + dir[2] * dir[2];
zdist = std::sqrt(zdist); zdist = std::sqrt(zdist);
len = std::sqrt(len); 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 radius = cone->radius;
FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h); 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) ...@@ -156,7 +156,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Cylinder* cylinder, 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())); 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; if (dir [2] > eps) support[2] = half_h;
else if (dir [2] < -eps) support[2] = -half_h; else if (dir [2] < -eps) support[2] = -half_h;
else support[2] = 0; else support[2] = 0;
......
...@@ -96,7 +96,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t ...@@ -96,7 +96,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t
std::vector<Vec3f> result(36); std::vector<Vec3f> result(36);
const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; 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 edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
FCL_REAL a = edge_size; FCL_REAL a = edge_size;
FCL_REAL b = m * edge_size; FCL_REAL b = m * edge_size;
...@@ -153,7 +153,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) ...@@ -153,7 +153,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
{ {
std::vector<Vec3f> result(7); 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 r2 = cone.radius * 2 / sqrt(3.0);
FCL_REAL a = 0.5 * r2; FCL_REAL a = 0.5 * r2;
FCL_REAL b = cone.radius; FCL_REAL b = cone.radius;
...@@ -174,7 +174,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& ...@@ -174,7 +174,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f&
{ {
std::vector<Vec3f> result(12); 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 r2 = cylinder.radius * 2 / sqrt(3.0);
FCL_REAL a = 0.5 * r2; FCL_REAL a = 0.5 * r2;
FCL_REAL b = cylinder.radius; FCL_REAL b = cylinder.radius;
...@@ -277,7 +277,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv) ...@@ -277,7 +277,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
const Matrix3f& R = tf.getRotation(); const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation(); 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.max_ = T + v_delta;
bv.min_ = T - v_delta; bv.min_ = T - v_delta;
} }
...@@ -288,9 +288,9 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) ...@@ -288,9 +288,9 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
const Matrix3f& R = tf.getRotation(); const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation(); 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 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) + 0.5 * fabs(R(1, 2) * s.lz); 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) + 0.5 * fabs(R(2, 2) * s.lz); 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); Vec3f v_delta(x_range, y_range, z_range);
bv.max_ = T + v_delta; bv.max_ = T + v_delta;
...@@ -303,9 +303,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& b ...@@ -303,9 +303,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& b
const Matrix3f& R = tf.getRotation(); const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation(); 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 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) + 0.5 * fabs(R(1, 2) * s.lz); 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) + 0.5 * fabs(R(2, 2) * s.lz); FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);