geometric_shapes.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef HPP_FCL_GEOMETRIC_SHAPES_H
#define HPP_FCL_GEOMETRIC_SHAPES_H

#include <boost/math/constants/constants.hpp>

#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/data_types.h>
#include <string.h>

namespace hpp
{
namespace fcl
{

class ShapeBase : public CollisionGeometry
{
public:
  ShapeBase() {}

  virtual ~ShapeBase () {};

  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};


class TriangleP : public ShapeBase
{
public:
  TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
  {
  }

  void computeLocalAABB();
  
  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }

  Vec3f a, b, c;
};

class Box : public ShapeBase
{
public:
  Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2)
  {
  }

  Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2) 
  {
  }

  Box() {}

  Vec3f halfSide;

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_BOX; }

  FCL_REAL computeVolume() const
  {
    return 8*halfSide.prod();
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
    Vec3f s (halfSide.cwiseAbs2() * V);
    return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
  }
};

class Sphere : public ShapeBase
{
public:
  Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
  {
  }
  
  FCL_REAL radius;

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL I = 0.4 * radius * radius * computeVolume();
    return I * Matrix3f::Identity();
  }

  FCL_REAL computeVolume() const
  {
    return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius / 3;
  }
};

class Capsule : public ShapeBase
{
public:
  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
  {
    halfLength = lz_/2;
  }

  FCL_REAL radius;

  FCL_REAL halfLength;

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }

  FCL_REAL computeVolume() const
  {
    return boost::math::constants::pi<FCL_REAL>() * radius * radius *((halfLength * 2) + radius * 4/3.0);
  }

  Matrix3f computeMomentofInertia() const
  {
    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 h2 = halfLength * halfLength;
    FCL_REAL r2 = radius * radius;
    FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
    FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;

    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
  }
  
};

class Cone : public ShapeBase
{
public:
  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
  {
    halfLength = lz_/2;
  }

  FCL_REAL radius;

  FCL_REAL halfLength;

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_CONE; }

  FCL_REAL computeVolume() const
  {
    return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
    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,
                          0, ix, 0,
                          0, 0, iz).finished();
  }

  Vec3f computeCOM() const
  {
    return Vec3f(0, 0, -0.5 * halfLength);
  }
};

class Cylinder : public ShapeBase
{
public:
  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
  {
    halfLength = lz_/2;
  }
  
  FCL_REAL radius;

  FCL_REAL halfLength;

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }

  FCL_REAL computeVolume() const
  {
    return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2);
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
    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,
                          0, 0, iz).finished();
  }
};

class ConvexBase : public ShapeBase
{
public:

  virtual ~ConvexBase();

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }

  Vec3f* points;
  int num_points;

  struct Neighbors
  {
    unsigned char count_;
    unsigned int* n_;

    unsigned char const& count () const { return count_; }
    unsigned int      & operator[] (int i)       { assert(i<count_); return n_[i]; }
    unsigned int const& operator[] (int i) const { assert(i<count_); return n_[i]; }
  };
  Neighbors* neighbors;

  Vec3f center;

protected:
  ConvexBase(bool ownStorage, Vec3f* points_, int num_points_);

  ConvexBase(const ConvexBase& other);

  unsigned int* nneighbors_;

  bool own_storage_;

private:
  void computeCenter();
};

template <typename PolygonT> class Convex;

class Halfspace : public ShapeBase
{
public:
  Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
  {
    unitNormalTest();
  }

  Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
  {
    unitNormalTest();
  }

  Halfspace() : ShapeBase(), n(1, 0, 0), d(0)
  {
  }

  FCL_REAL signedDistance(const Vec3f& p) const
  {
    return n.dot(p) - d;
  }

  FCL_REAL distance(const Vec3f& p) const
  {
    return std::abs(n.dot(p) - d);
  }

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
  
  Vec3f n;
  
  FCL_REAL d;

protected:

  void unitNormalTest();
};

class Plane : public ShapeBase
{
public:
  Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) 
  { 
    unitNormalTest(); 
  }
  
  Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
  {
    unitNormalTest();
  }

  Plane() : ShapeBase(), n(1, 0, 0), d(0)
  {}

  FCL_REAL signedDistance(const Vec3f& p) const
  {
    return n.dot(p) - d;
  }

  FCL_REAL distance(const Vec3f& p) const
  {
    return std::abs(n.dot(p) - d);
  }

  void computeLocalAABB();

  NODE_TYPE getNodeType() const { return GEOM_PLANE; }

  Vec3f n;

  FCL_REAL d;

protected:
  
  void unitNormalTest();
};

}

} // namespace hpp

#endif