Merge pull request #136 from jmirabel/devel
[GJK/EPA] Fix bugs + Treat sphere as point and capsule as line segment.
python/gjk.cc
0 → 100644
| ... | ... | @@ -67,6 +67,18 @@ template <> struct shape_traits<Box> : shape_traits_base |
| }; | ||
| }; | ||
| template <> struct shape_traits<Sphere> : shape_traits_base | ||
| { | ||
| enum { NeedNormalizedDir = false | ||
| }; | ||
| }; | ||
| template <> struct shape_traits<Capsule> : shape_traits_base | ||
| { | ||
| enum { NeedNormalizedDir = false | ||
| }; | ||
| }; | ||
| template <> struct shape_traits<Cone> : shape_traits_base | ||
| { | ||
| enum { NeedNormalizedDir = false | ||
| ... | ... | @@ -111,60 +123,76 @@ inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support) |
| support.noalias() = (dir.array() > 0).select(box->halfSide, -box->halfSide); | ||
| } | ||
| inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support) | ||
| inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support) | ||
| { | ||
| support = dir * sphere->radius; | ||
| support.setZero(); | ||
| } | ||
| inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support) | ||
| { | ||
| support = capsule->radius * dir; | ||
| if (dir[2] > 0) support[2] += capsule->halfLength; | ||
| else support[2] -= capsule->halfLength; | ||
| support.head<2>().setZero(); | ||
| if (dir[2] > 0) support[2] = capsule->halfLength; | ||
| else support[2] = - capsule->halfLength; | ||
| } | ||
| void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support) | ||
| { | ||
| // TODO (Joseph Mirabel) | ||
| // this assumes that the cone radius is, for -0.5 < z < 0.5: | ||
| // (lz/2 - z) * radius / lz | ||
| // | ||
| // I did not change the code myself. However, I think it should be revised. | ||
| // 1. It can be optimized. | ||
| // 2. I am not sure this is what conePlaneIntersect and coneHalfspaceIntersect | ||
| // assumes... | ||
| // The cone radius is, for -h < z < h, (h - z) * r / (2*h) | ||
| static const FCL_REAL inflate = 1.00001; | ||
| FCL_REAL h = cone->halfLength; | ||
| FCL_REAL r = cone->radius; | ||
| if (dir.head<2>().isZero()) { | ||
| support.head<2>().setZero(); | ||
| if (dir[2] > 0) | ||
| support[2] = h; | ||
| else | ||
| support[2] = - inflate * h; | ||
| return; | ||
| } | ||
| FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; | ||
| FCL_REAL len = zdist + dir[2] * dir[2]; | ||
| zdist = std::sqrt(zdist); | ||
| len = std::sqrt(len); | ||
| 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); | ||
| if (dir[2] <= 0) { | ||
| FCL_REAL rad = r / zdist; | ||
| support.head<2>() = rad * dir.head<2>(); | ||
| support[2] = -h; | ||
| return; | ||
| } | ||
| len = std::sqrt(len); | ||
| FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h); | ||
| if(dir[2] > len * sin_a) | ||
| support = Vec3f(0, 0, half_h); | ||
| else if(zdist > 0) | ||
| { | ||
| FCL_REAL rad = radius / zdist; | ||
| support = Vec3f(rad * dir[0], rad * dir[1], -half_h); | ||
| support << 0, 0, h; | ||
| else { | ||
| FCL_REAL rad = r / zdist; | ||
| support.head<2>() = rad * dir.head<2>(); | ||
| support[2] = -h; | ||
| } | ||
| else | ||
| support = Vec3f(0, 0, -half_h); | ||
| } | ||
| void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support) | ||
| { | ||
| static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon())); | ||
| // The inflation makes the object look strictly convex to GJK and EPA. This | ||
| // helps solving particular cases (e.g. a cylinder with itself at the same | ||
| // position...) | ||
| static const FCL_REAL inflate = 1.00001; | ||
| 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; | ||
| if (dir.head<2>().isZero()) | ||
| FCL_REAL r = cylinder->radius; | ||
| if (dir.head<2>() == Eigen::Matrix<FCL_REAL,2,1>::Zero()) half_h *= inflate; | ||
| if (dir[2] > 0) support[2] = half_h; | ||
| else if (dir[2] < 0) support[2] = -half_h; | ||
| else { support[2] = 0; r *= inflate; } | ||
| if (dir.head<2>() == Eigen::Matrix<FCL_REAL,2,1>::Zero()) | ||
| support.head<2>().setZero(); | ||
| else | ||
| support.head<2>() = dir.head<2>().normalized() * cylinder->radius; | ||
| assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps); | ||
| support.head<2>() = dir.head<2>().normalized() * r; | ||
| assert (fabs (support [0] * dir [1] - support [1] * dir [0]) | ||
| < sqrt(std::numeric_limits<FCL_REAL>::epsilon())); | ||
| } | ||
| void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support) | ||
| ... | ... | @@ -276,8 +304,10 @@ void getSupportFuncTpl (const MinkowskiDiff& md, |
| } | ||
| template <typename Shape0> | ||
| MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity) | ||
| MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity, | ||
| Eigen::Array<FCL_REAL, 1, 2>& inflation) | ||
| { | ||
| inflation[1] = 0; | ||
| switch(s1->getNodeType()) | ||
| { | ||
| case GEOM_TRIANGLE: | ||
| ... | ... | @@ -287,9 +317,11 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, |
| if (identity) return getSupportFuncTpl<Shape0, Box, true >; | ||
| else return getSupportFuncTpl<Shape0, Box, false>; | ||
| case GEOM_SPHERE: | ||
| inflation[1] = static_cast<const Sphere*>(s1)->radius; | ||
| if (identity) return getSupportFuncTpl<Shape0, Sphere, true >; | ||
| else return getSupportFuncTpl<Shape0, Sphere, false>; | ||
| case GEOM_CAPSULE: | ||
| inflation[1] = static_cast<const Capsule*>(s1)->radius; | ||
| if (identity) return getSupportFuncTpl<Shape0, Capsule, true >; | ||
| < |