38 #ifndef HPP_FCL_SHAPE_CONVEX_HXX 39 #define HPP_FCL_SHAPE_CONVEX_HXX 49 template <
typename PolygonT>
51 PolygonT* polygons_,
int num_polygons_) :
ConvexBase(),
53 num_polygons (num_polygons_)
59 template <
typename PolygonT>
72 template <
typename PolygonT>
78 template <
typename PolygonT>
81 typedef typename PolygonT::size_type size_type;
82 typedef typename PolygonT::index_type index_type;
87 C_canonical << 1/60.0, 1/120.0, 1/120.0,
88 1/120.0, 1/60.0, 1/120.0,
89 1/120.0, 1/120.0, 1/60.0;
93 const PolygonT& polygon (
polygons[i]);
96 Vec3f plane_center(0,0,0);
97 for(size_type j = 0; j < polygon.size(); ++j)
98 plane_center +=
points[polygon[j]];
99 plane_center /= polygon.size();
102 const Vec3f& v3 = plane_center;
103 for(size_type j = 0; j < polygon.size(); ++j)
105 index_type e_first = polygon[j];
106 index_type e_second = polygon[(j+1)%polygon.size()];
109 Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
110 C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
114 return C.trace() * Matrix3f::Identity() - C;
117 template <
typename PolygonT>
120 typedef typename PolygonT::size_type size_type;
121 typedef typename PolygonT::index_type index_type;
127 const PolygonT& polygon (
polygons[i]);
129 Vec3f plane_center(0,0,0);
130 for(size_type j = 0; j < polygon.size(); ++j)
131 plane_center +=
points[polygon[j]];
132 plane_center /= polygon.size();
135 const Vec3f& v3 = plane_center;
136 for(size_type j = 0; j < polygon.size(); ++j)
138 index_type e_first = polygon[j];
139 index_type e_second = polygon[(j+1)%polygon.size()];
142 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
144 com += (
points[e_first] +
points[e_second] + plane_center) * d_six_vol;
148 return com / (vol * 4);
151 template <
typename PolygonT>
154 typedef typename PolygonT::size_type size_type;
155 typedef typename PolygonT::index_type index_type;
160 const PolygonT& polygon (
polygons[i]);
163 Vec3f plane_center(0,0,0);
164 for(size_type j = 0; j < polygon.size(); ++j)
165 plane_center +=
points[polygon[j]];
166 plane_center /= polygon.size();
169 const Vec3f& v3 = plane_center;
170 for(size_type j = 0; j < polygon.size(); ++j)
172 index_type e_first = polygon[j];
173 index_type e_second = polygon[(j+1)%polygon.size()];
176 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
184 template <
typename PolygonT>
189 typedef typename PolygonT::size_type size_type;
190 typedef typename PolygonT::index_type index_type;
191 std::vector<std::set<index_type> > nneighbors (
num_points);
192 unsigned int c_nneighbors = 0;
196 const PolygonT& polygon (
polygons[l]);
197 size_type n = polygon.size();
199 for(size_type j = 0; j < polygon.size(); ++j)
201 size_type i = (j==0 ) ? n-1 : j-1;
202 size_type k = (j==n-1) ? 0 : j+1;
203 index_type pi = polygon[i],
207 if (nneighbors[pj].count(pi) == 0) {
209 nneighbors[pj].insert(pi);
211 if (nneighbors[pj].count(pk) == 0) {
213 nneighbors[pj].insert(pk);
222 if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
223 throw std::logic_error (
"Too many neighbors.");
224 n.
count_ = (
unsigned char)nneighbors[i].size();
226 p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
228 assert (p_nneighbors ==
nneighbors_ + c_nneighbors);
FCL_REAL computeVolume() const
compute the volume
Definition: convex.hxx:152
bool own_storage_
Definition: geometric_shapes.h:346
Main namespace.
Definition: AABB.h:43
Definition: geometric_shapes.h:311
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
void fillNeighbors()
Definition: convex.hxx:185
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:118
void initialize(bool ownStorage, Vec3f *points_, int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
~Convex()
Definition: convex.hxx:73
PolygonT * polygons
An array of PolygonT object. PolygonT should contains a list of vertices for each polygon...
Definition: convex.h:80
double FCL_REAL
Definition: data_types.h:66
unsigned int * n_
Definition: geometric_shapes.h:314
int num_polygons
Definition: convex.h:81
unsigned char count_
Definition: geometric_shapes.h:313
int num_points
Definition: geometric_shapes.h:309
Convex()
Construct an uninitialized convex object.
Definition: convex.h:57
Base for convex polytope.
Definition: geometric_shapes.h:281
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:79
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Convex polytope.
Definition: convex.h:53
Neighbors * neighbors
Definition: geometric_shapes.h:323
unsigned int * nneighbors_
Definition: geometric_shapes.h:344
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:308