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

[GJK] Avoid normalization for Cylinder, Convex and Cone.

parent 04bc35c3
...@@ -67,6 +67,24 @@ template <> struct shape_traits<Box> : shape_traits_base ...@@ -67,6 +67,24 @@ template <> struct shape_traits<Box> : shape_traits_base
}; };
}; };
template <> struct shape_traits<Cone> : shape_traits_base
{
enum { NeedNormalizedDir = false
};
};
template <> struct shape_traits<Cylinder> : shape_traits_base
{
enum { NeedNormalizedDir = false
};
};
template <> struct shape_traits<Convex> : shape_traits_base
{
enum { NeedNormalizedDir = false
};
};
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support) void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support)
{ {
FCL_REAL dota = dir.dot(triangle->a); FCL_REAL dota = dir.dot(triangle->a);
...@@ -138,17 +156,14 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support) ...@@ -138,17 +156,14 @@ 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 zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
FCL_REAL half_h = cylinder->lz * 0.5; FCL_REAL half_h = cylinder->lz * 0.5;
if(zdist == 0.0) if (dir [2] > eps) support[2] = half_h;
support = Vec3f(0, 0, (dir[2]>0)? half_h:-half_h); else if (dir [2] < -eps) support[2] = -half_h;
else { else support[2] = 0;
FCL_REAL d = cylinder->radius / zdist; if (dir.head<2>().isZero())
FCL_REAL z (0.); support.head<2>().setZero();
if (dir [2] > eps) z = half_h; else
else if (dir [2] < -eps) z = -half_h; support.head<2>() = dir.head<2>().normalized() * cylinder->radius;
support << d * dir.head<2>(), z;
}
assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps); assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps);
} }
......
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