/*

 * 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.

 */

/** \author Jia Pan */

#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 {

/// @brief Base class for all basic geometric shapes

 public:

  ///  \brief Copy constructor

  ShapeBase& operator=(const ShapeBase& other) = default;

  /// @brief Get object type: a geometric shape

};

/// @defgroup Geometric_Shapes Geometric shapes

/// Classes of different types of geometric shapes.

/// @{

/// @brief Triangle stores the points instead of only indices of points

class HPP_FCL_DLLAPI TriangleP : public ShapeBase {

 public:

  /// @brief Clone *this into a new TriangleP

  /// @brief virtual function of compute AABB in local coordinate

  void computeLocalAABB();

  //  std::pair<ShapeBase*, Transform3f> inflated(const FCL_REAL value) const {

  //    if (value == 0) return std::make_pair(new TriangleP(*this),

  //    Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize();

  //    BC.normalize();

  //    CA.normalize();

  //

  //    Vec3f new_a(a + value * Vec3f(-AB + CA).normalized());

  //    Vec3f new_b(b + value * Vec3f(-BC + AB).normalized());

  //    Vec3f new_c(c + value * Vec3f(-CA + BC).normalized());

  //

  //    return std::make_pair(new TriangleP(new_a, new_b, new_c),

  //    Transform3f());

  //  }

  //

  //  FCL_REAL minInflationValue() const

  //  {

  //    return (std::numeric_limits<FCL_REAL>::max)(); // TODO(jcarpent):

  //    implement

  //  }

  Vec3f a, b, c;

 private:

  }

 public:

};

/// @brief Center at zero point, axis aligned box

class HPP_FCL_DLLAPI Box : public ShapeBase {

 public:

  }

  /// @brief Clone *this into a new Box

  /// @brief Default constructor

  /// @brief box side half-length

  Vec3f halfSide;

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a box

  }

  /// \brief Inflate the box by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated box and the related transform to account for the

  /// change of shape frame

                                     << "is two small. It should be at least: "

                                     << minInflationValue(),

                           std::invalid_argument);

  }

 private:

  }

 public:

};

/// @brief Center at zero point sphere

class HPP_FCL_DLLAPI Sphere : public ShapeBase {

 public:

  /// @brief Default constructor

  /// @brief Clone *this into a new Sphere

  /// @brief Radius of the sphere

  FCL_REAL radius;

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a sphere

  }

  }

  /// \brief Inflate the sphere by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated sphere and the related transform to account for

  /// the change of shape frame

          "value (" << value << ") is two small. It should be at least: "

                    << minInflationValue(),

          std::invalid_argument);

  }

 private:

  }

 public:

};

/// @brief Ellipsoid centered at point zero

class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase {

 public:

  /// @brief Default constructor

  /// @brief Clone *this into a new Ellipsoid

  /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2

  /// + z^2/rz^2 = 1)

  Vec3f radii;

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: an ellipsoid

  }

  }

  /// \brief Inflate the ellipsoid by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated ellipsoid and the related transform to account for

  /// the change of shape frame

          "value (" << value << ") is two small. It should be at least: "

                    << minInflationValue(),

          std::invalid_argument);

  }

 private:

  }

 public:

};

/// @brief Capsule

/// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$

/// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule

/// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$.

class HPP_FCL_DLLAPI Capsule : public ShapeBase {

 public:

  /// @brief Default constructor

  /// @brief Clone *this into a new Capsule

  /// @brief Radius of capsule

  FCL_REAL radius;

  /// @brief Half Length along z axis

  FCL_REAL halfLength;

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a capsule

  }

  }

  /// \brief Inflate the capsule by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated capsule and the related transform to account for

  /// the change of shape frame

          "value (" << value << ") is two small. It should be at least: "

                    << minInflationValue(),

          std::invalid_argument);

  }

 private:

  }

 public:

};

/// @brief Cone

/// The base of the cone is at \f$ z = - halfLength \f$ and the top is at

/// \f$ z = halfLength \f$.

class HPP_FCL_DLLAPI Cone : public ShapeBase {

 public:

  /// @brief Default constructor

  /// @brief Clone *this into a new Cone

  /// @brief Radius of the cone

  FCL_REAL radius;

  /// @brief Half Length along z axis

  FCL_REAL halfLength;

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a cone

  }

  }

  /// \brief Inflate the cone by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated cone and the related transform to account for the

  /// change of shape frame

          "value (" << value << ") is two small. It should be at least: "

                    << minInflationValue(),

          std::invalid_argument);

    // tan(alpha) = 2*halfLength/radius;

  }

 private:

  }

 public:

};

/// @brief Cylinder along Z axis.

/// The cylinder is defined at its centroid.

class HPP_FCL_DLLAPI Cylinder : public ShapeBase {

 public:

  /// @brief Default constructor

  }

  /// @brief Clone *this into a new Cylinder

  /// @brief Radius of the cylinder

  FCL_REAL radius;

  /// @brief Half Length along z axis

  FCL_REAL halfLength;

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a cylinder

  }

  }

  /// \brief Inflate the cylinder by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated cylinder and the related transform to account for

  /// the change of shape frame

          "value (" << value << ") is two small. It should be at least: "

                    << minInflationValue(),

          std::invalid_argument);

  }

 private:

  }

 public:

};

/// @brief Base for convex polytope.

/// @note Inherited classes are responsible for filling ConvexBase::neighbors;

class HPP_FCL_DLLAPI ConvexBase : public ShapeBase {

 public:

  /// @brief Build a convex hull based on Qhull library

  /// and store the vertices and optionally the triangles

  /// \param points, num_points the points whose convex hull should be computed.

  /// \param keepTriangles if \c true, returns a Convex<Triangle> object which

  ///        contains the triangle of the shape.

  /// \param qhullCommand the command sent to qhull.

  ///        - if \c keepTriangles is \c true, this parameter should include

  ///          "Qt". If \c NULL, "Qt" is passed to Qhull.

  ///        - if \c keepTriangles is \c false, an empty string is passed to

  ///          Qhull.

  /// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set

  ///       to \c ON.

  static ConvexBase* convexHull(const Vec3f* points, unsigned int num_points,

                                bool keepTriangles,

                                const char* qhullCommand = NULL);

  virtual ~ConvexBase();

  ///  @brief Clone (deep copy).

    }

  }

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a conex polytope

  /// @brief An array of the points of the polygon.

  Vec3f* points;

  unsigned int num_points;

  struct HPP_FCL_DLLAPI Neighbors {

    unsigned char count_;

    unsigned int* n_;

    }

    }

      }

    }

  };

  /// Neighbors of each vertex.

  /// It is an array of size num_points. For each vertex, it contains the number

  /// of neighbors and a list of indices to them.

  Neighbors* neighbors;

  /// @brief center of the convex polytope, this is used for collision: center

  /// is guaranteed in the internal of the polytope (as it is convex)

  Vec3f center;

 protected:

  /// @brief Construct an uninitialized convex object

  /// Initialization is done with ConvexBase::initialize.

        points(NULL),

        num_points(0),

        neighbors(NULL),

        nneighbors_(NULL),

  /// @brief Initialize the points of the convex shape

  /// This also initializes the ConvexBase::center.

  ///

  /// \param ownStorage weither the ConvexBase owns the data.

  /// \param points_ list of 3D points  ///

  /// \param num_points_ number of 3D points

  void initialize(bool ownStorage, Vec3f* points_, unsigned int num_points_);

  /// @brief Set the points of the convex shape.

  ///

  /// \param ownStorage weither the ConvexBase owns the data.

  /// \param points_ list of 3D points  ///

  /// \param num_points_ number of 3D points

  void set(bool ownStorage, Vec3f* points_, unsigned int num_points_);

  /// @brief Copy constructor

  /// Only the list of neighbors is copied.

  ConvexBase(const ConvexBase& other);

  unsigned int* nneighbors_;

  bool own_storage_;

 private:

  void computeCenter();

 private:

    }

    }

  }

 public:

};

template <typename PolygonT>

class Convex;

/// @brief Half Space: this is equivalent to the Plane in ODE. The separation

/// plane is defined as n * x = d; Points in the negative side of the separation

/// plane (i.e. {x | n * x < d}) are inside the half space and points in the

/// positive side of the separation plane (i.e. {x | n * x > d}) are outside the

/// half space

class HPP_FCL_DLLAPI Halfspace : public ShapeBase {

 public:

  /// @brief Construct a half space with normal direction and offset

  /// @brief Construct a plane with normal direction and offset

  }

  /// @brief operator =

  }

  /// @brief Clone *this into a new Halfspace

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

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a half space

  }

  /// \brief Inflate the cylinder by an amount given by value

  ///

  /// \param[in] value of the shape inflation.

  ///

  /// \returns a new inflated cylinder and the related transform to account for

  /// the change of shape frame

          "value (" << value << ") is two small. It should be at least: "

                    << minInflationValue(),

          std::invalid_argument);

  }

  /// @brief Plane normal

  Vec3f n;

  /// @brief Plane offset

  FCL_REAL d;

 protected:

  /// @brief Turn non-unit normal into unit

  void unitNormalTest();

 private:

  }

 public:

};

/// @brief Infinite plane

class HPP_FCL_DLLAPI Plane : public ShapeBase {

 public:

  /// @brief Construct a plane with normal direction and offset

  /// @brief Construct a plane with normal direction and offset

  }

  /// @brief operator =

  }

  /// @brief Clone *this into a new Plane

  /// @brief Compute AABB

  void computeLocalAABB();

  /// @brief Get node type: a plane

  /// @brief Plane normal

  Vec3f n;

  /// @brief Plane offset

  FCL_REAL d;

 protected:

  /// @brief Turn non-unit normal into unit

  void unitNormalTest();

 private:

  }

 public:

};

}  // namespace fcl

}  // namespace hpp

#endif