Commit 07ec65b4 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Convex] Make class constructible from triangles.

parent e8a5f65d
......@@ -55,29 +55,35 @@ typedef boost::int32_t FCL_INT32;
/// @brief Triangle with 3 indices for points
class Triangle
{
/// @brief indices for each vertex of triangle
std::size_t vids[3];
public:
typedef std::size_t index_type;
typedef int size_type;
/// @brief Default constructor
Triangle() {}
/// @brief Create a triangle with given vertex indices
Triangle(std::size_t p1, std::size_t p2, std::size_t p3)
Triangle(index_type p1, index_type p2, index_type p3)
{
set(p1, p2, p3);
}
/// @brief Set the vertex indices of the triangle
inline void set(std::size_t p1, std::size_t p2, std::size_t p3)
inline void set(index_type p1, index_type p2, index_type p3)
{
vids[0] = p1; vids[1] = p2; vids[2] = p3;
}
/// @access the triangle index
inline std::size_t operator[](int i) const { return vids[i]; }
inline index_type operator[](int i) const { return vids[i]; }
inline std::size_t& operator[](int i) { return vids[i]; }
inline index_type& operator[](int i) { return vids[i]; }
static inline size_type size() { return 3; }
private:
/// @brief indices for each vertex of triangle
index_type vids[3];
};
}
......
/*
* 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_SHAPE_CONVEX_H
#define HPP_FCL_SHAPE_CONVEX_H
#include <hpp/fcl/shape/geometric_shapes.h>
namespace hpp
{
namespace fcl
{
/// @brief Convex polytope
/// @tparam PolygonT the polygon class. It must have method \c size() and
/// \c operator[](int i)
template <typename PolygonT>
class Convex : public ConvexBase
{
public:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
/// \param points_ list of 3D points
/// \param num_points_ number of 3D points
/// \param polygons_ \copydoc Convex::polygons
/// \param num_polygons_ the number of polygons.
/// \note num_polygons_ is not the allocated size of polygons_.
Convex(Vec3f* points_, int num_points_,
PolygonT* polygons_, int num_polygons_);
/// @brief Copy constructor
/// Only the list of neighbors is copied.
Convex(const Convex& other);
~Convex();
/// @brief An array of PolygonT object.
/// PolygonT should contains a list of vertices for each polygon,
/// in counter clockwise order.
PolygonT* polygons;
int num_polygons;
/// based on http://number-none.com/blow/inertia/bb_inertia.doc
Matrix3f computeMomentofInertia() const;
Vec3f computeCOM() const;
FCL_REAL computeVolume() const;
protected:
void fillNeighbors();
};
}
} // namespace hpp
#include <hpp/fcl/shape/details/convex.hxx>
#endif
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, CNRS - LAAS
* 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 Joseph Mirabel */
#ifndef HPP_FCL_SHAPE_CONVEX_HXX
#define HPP_FCL_SHAPE_CONVEX_HXX
#include <set>
namespace hpp
{
namespace fcl
{
template <typename PolygonT>
Convex<PolygonT>::Convex(Vec3f* points_,
int num_points_,
PolygonT* polygons_,
int num_polygons_) :
ConvexBase(points_, num_points_),
polygons (polygons_),
num_polygons (num_polygons_)
{
fillNeighbors();
}
template <typename PolygonT>
Convex<PolygonT>::Convex(const Convex<PolygonT>& other) :
ConvexBase (other),
polygons (other.polygons),
num_polygons (other.num_polygons)
{}
template <typename PolygonT>
Convex<PolygonT>::~Convex()
{}
template <typename PolygonT>
Matrix3f Convex<PolygonT>::computeMomentofInertia() const
{
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
Matrix3f C = Matrix3f::Zero();
Matrix3f C_canonical;
C_canonical << 1/60.0, 1/120.0, 1/120.0,
1/120.0, 1/60.0, 1/120.0,
1/120.0, 1/120.0, 1/60.0;
for(int i = 0; i < num_polygons; ++i)
{
const PolygonT& polygon (polygons[i]);
// compute the center of the polygon
Vec3f plane_center(0,0,0);
for(size_type j = 0; j < polygon.size(); ++j)
plane_center += points[polygon[j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
const Vec3f& v3 = plane_center;
for(size_type j = 0; j < polygon.size(); ++j)
{
index_type e_first = polygon[j];
index_type e_second = polygon[(j+1)%polygon.size()];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document
C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
}
return C.trace() * Matrix3f::Identity() - C;
}
template <typename PolygonT>
Vec3f Convex<PolygonT>::computeCOM() const
{
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
Vec3f com(0,0,0);
FCL_REAL vol = 0;
for(int i = 0; i < num_polygons; ++i)
{
const PolygonT& polygon (polygons[i]);
// compute the center of the polygon
Vec3f plane_center(0,0,0);
for(size_type j = 0; j < polygon.size(); ++j)
plane_center += points[polygon[j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
const Vec3f& v3 = plane_center;
for(size_type j = 0; j < polygon.size(); ++j)
{
index_type e_first = polygon[j];
index_type e_second = polygon[(j+1)%polygon.size()];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
}
}
return com / (vol * 4); // here we choose zero as the reference
}
template <typename PolygonT>
FCL_REAL Convex<PolygonT>::computeVolume() const
{
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
FCL_REAL vol = 0;
for(int i = 0; i < num_polygons; ++i)
{
const PolygonT& polygon (polygons[i]);
// compute the center of the polygon
Vec3f plane_center(0,0,0);
for(size_type j = 0; j < polygon.size(); ++j)
plane_center += points[polygon[j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape
const Vec3f& v3 = plane_center;
for(size_type j = 0; j < polygon.size(); ++j)
{
index_type e_first = polygon[j];
index_type e_second = polygon[(j+1)%polygon.size()];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
}
}
return vol / 6;
}
template <typename PolygonT>
void Convex<PolygonT>::fillNeighbors()
{
neighbors = new Neighbors[num_points];
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
std::vector<std::set<index_type> > nneighbors (num_points);
unsigned int c_nneighbors = 0;
for (int l = 0; l < num_polygons; ++l)
{
const PolygonT& polygon (polygons[l]);
size_type n = polygon.size();
for(size_type j = 0; j < polygon.size(); ++j)
{
size_type i = (j==0 ) ? n-1 : j-1;
size_type k = (j==n-1) ? 0 : j+1;
index_type pi = polygon[i+1],
pj = polygon[j+1],
pk = polygon[k+1];
// Update neighbors of pj;
if (nneighbors[pj].count(pi) == (unsigned int)0) {
c_nneighbors++;
nneighbors[pj].insert(pi);
}
if (nneighbors[pj].count(pk) == (unsigned int)0) {
c_nneighbors++;
nneighbors[pj].insert(pk);
}
}
}
nneighbors_ = new unsigned int[c_nneighbors];
unsigned int* p_nneighbors = nneighbors_;
for (int i = 0; i < num_points; ++i) {
Neighbors& n = neighbors[i];
if (nneighbors[i].size() >= std::numeric_limits<unsigned char>::max())
throw std::logic_error ("Too many neighbors.");
n.count_ = (unsigned char)nneighbors[i].size();
n.n_ = p_nneighbors;
p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
}
assert (p_nneighbors == nneighbors_ + c_nneighbors);
}
}
} // namespace hpp
#endif
......@@ -262,53 +262,12 @@ public:
}
};
/// @brief Convex polytope
class Convex : public ShapeBase
/// @brief Base for convex polytope.
/// @note Inherited classes are responsible for filling ConvexBase::neighbors;
class ConvexBase : public ShapeBase
{
public:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
/// \param points_ list of 3D points
/// \param num_points_ number of 3D points
/// \param polygons_ \copydoc Convex::polygons
/// \param num_polygons_ the number of polygons.
/// \note num_polygons_ is not the allocated size of polygons_.
Convex(Vec3f* points_,
int num_points_,
int* polygons_,
int num_polygons_) :
ShapeBase(),
points (points_),
num_points (num_points_),
polygons (polygons_),
num_polygons (num_polygons_)
{
initialize();
}
/// @brief Copy constructor
/// Only the list of neighbors is copied.
Convex(const Convex& other) :
ShapeBase (other),
points (other.points),
num_points (other.num_points),
polygons (other.polygons),
num_polygons (other.num_polygons),
center (other.center)
{
neighbors = new Neighbors[num_points];
memcpy(neighbors, other.neighbors, sizeof(Neighbors) * num_points);
int c_nneighbors = 0;
for (int i = 0; i < num_points; ++i) c_nneighbors += neighbors[i].count();
nneighbors_ = new unsigned int[c_nneighbors];
memcpy(nneighbors_, other.nneighbors_, sizeof(unsigned int) * c_nneighbors);
}
~Convex()
{
delete [] neighbors;
delete [] nneighbors_;
}
virtual ~ConvexBase();
/// @brief Compute AABB
void computeLocalAABB();
......@@ -316,26 +275,18 @@ public:
/// @brief Get node type: a conex polytope
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
/// @brief An array of indices to the points of each polygon.
/// It should be the number of vertices
/// followed by that amount of indices to "points" in counter clockwise order.
int* polygons;
int num_polygons;
/// @brief An array of the points of the polygon.
Vec3f* points;
int num_points;
class Neighbors
struct Neighbors
{
private:
unsigned char count_;
unsigned int* n_;
friend class Convex;
public:
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]; }
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 of each vertex.
/// It is an array of size num_points. For each vertex, it contains the number
......@@ -345,120 +296,20 @@ public:
/// @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;
/// based on http://number-none.com/blow/inertia/bb_inertia.doc
Matrix3f computeMomentofInertia() const
{
Matrix3f C = Matrix3f::Zero();
Matrix3f C_canonical;
C_canonical << 1/60.0, 1/120.0, 1/120.0,
1/120.0, 1/60.0, 1/120.0,
1/120.0, 1/120.0, 1/60.0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_polygons; ++i)
{
Vec3f plane_center(0,0,0);
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
plane_center += points[index[j]];
plane_center = plane_center * (1.0 / *points_in_poly);
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
const Vec3f& v3 = plane_center;
for(int j = 0; j < *points_in_poly; ++j)
{
int e_first = index[j];
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
}
return C.trace() * Matrix3f::Identity() - C;
}
Vec3f computeCOM() const
{
Vec3f com(0,0,0);
FCL_REAL vol = 0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_polygons; ++i)
{
Vec3f plane_center(0,0,0);
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
plane_center += points[index[j]];
plane_center = plane_center * (1.0 / *points_in_poly);
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
const Vec3f& v3 = plane_center;
for(int j = 0; j < *points_in_poly; ++j)
{
int e_first = index[j];
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
}
return com / (vol * 4); // here we choose zero as the reference
}
protected:
/// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
/// \param points_ list of 3D points
/// \param num_points_ number of 3D points
ConvexBase(Vec3f* points_, int num_points_);
FCL_REAL computeVolume() const
{
FCL_REAL vol = 0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_polygons; ++i)
{
Vec3f plane_center(0,0,0);
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
plane_center += points[index[j]];
plane_center = plane_center * (1.0 / *points_in_poly);
// compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape
const Vec3f& v3 = plane_center;
for(int j = 0; j < *points_in_poly; ++j)
{
int e_first = index[j];
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
}
points_in_poly += (*points_in_poly + 1);
index = points_in_poly + 1;
}
return vol / 6;
}
/// @brief Copy constructor
/// Only the list of neighbors is copied.
ConvexBase(const ConvexBase& other);
protected:
void initialize();
unsigned int* nneighbors_;
private:
unsigned int* nneighbors_;
void computeCenter();
};
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
......@@ -558,7 +409,6 @@ protected:
void unitNormalTest();
};
}
} // namespace hpp
......
......@@ -57,7 +57,7 @@ std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
std::vector<Ve