38 #ifndef HPP_FCL_SHAPE_CONVEX_HXX 39 #define HPP_FCL_SHAPE_CONVEX_HXX 49 template <
typename PolygonT>
51 PolygonT* polygons_,
unsigned int num_polygons_) :
ConvexBase(),
53 num_polygons (num_polygons_)
59 template <
typename PolygonT>
62 polygons (other.polygons),
63 num_polygons (other.num_polygons)
71 template <
typename PolygonT>
74 if (own_storage_)
delete [] polygons;
77 template <
typename PolygonT>
79 PolygonT* polygons_,
unsigned int num_polygons_)
85 num_polygons = num_polygons_;
91 template <
typename PolygonT>
94 typedef typename PolygonT::size_type size_type;
95 typedef typename PolygonT::index_type index_type;
100 C_canonical << 1/60.0, 1/120.0, 1/120.0,
101 1/120.0, 1/60.0, 1/120.0,
102 1/120.0, 1/120.0, 1/60.0;
104 for(
unsigned int i = 0; i < num_polygons; ++i)
106 const PolygonT & polygon = polygons[i];
109 Vec3f plane_center(0,0,0);
110 for(size_type j = 0; j < polygon.size(); ++j)
111 plane_center += points[polygon[j]];
112 plane_center /= polygon.size();
115 const Vec3f& v3 = plane_center;
116 for(size_type j = 0; j < polygon.size(); ++j)
118 index_type e_first = polygon[j];
119 index_type e_second = polygon[(j+1)%polygon.size()];
120 const Vec3f& v1 = points[e_first];
121 const Vec3f& v2 = points[e_second];
122 Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
123 C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
127 return C.trace() * Matrix3f::Identity() - C;
130 template <
typename PolygonT>
133 typedef typename PolygonT::size_type size_type;
134 typedef typename PolygonT::index_type index_type;
138 for(
unsigned int i = 0; i < num_polygons; ++i)
140 const PolygonT & polygon = polygons[i];
142 Vec3f plane_center(0,0,0);
143 for(size_type j = 0; j < polygon.size(); ++j)
144 plane_center += points[polygon[j]];
145 plane_center /= polygon.size();
148 const Vec3f& v3 = plane_center;
149 for(size_type j = 0; j < polygon.size(); ++j)
151 index_type e_first = polygon[j];
152 index_type e_second = polygon[(j+1)%polygon.size()];
153 const Vec3f& v1 = points[e_first];
154 const Vec3f& v2 = points[e_second];
155 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
157 com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
161 return com / (vol * 4);
164 template <
typename PolygonT>
167 typedef typename PolygonT::size_type size_type;
168 typedef typename PolygonT::index_type index_type;
171 for(
unsigned int i = 0; i < num_polygons; ++i)
173 const PolygonT & polygon = polygons[i];
176 Vec3f plane_center(0,0,0);
177 for(size_type j = 0; j < polygon.size(); ++j)
178 plane_center += points[polygon[j]];
179 plane_center /= polygon.size();
182 const Vec3f& v3 = plane_center;
183 for(size_type j = 0; j < polygon.size(); ++j)
185 index_type e_first = polygon[j];
186 index_type e_second = polygon[(j+1)%polygon.size()];
187 const Vec3f& v1 = points[e_first];
188 const Vec3f& v2 = points[e_second];
189 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
197 template <
typename PolygonT>
202 typedef typename PolygonT::size_type size_type;
203 typedef typename PolygonT::index_type index_type;
204 std::vector<std::set<index_type> > nneighbors (num_points);
205 unsigned int c_nneighbors = 0;
207 for (
unsigned int l = 0; l < num_polygons; ++l)
209 const PolygonT & polygon = polygons[l];
210 const size_type n = polygon.size();
212 for(size_type j = 0; j < polygon.size(); ++j)
214 size_type i = (j==0 ) ? n-1 : j-1;
215 size_type k = (j==n-1) ? 0 : j+1;
216 index_type pi = polygon[i],
220 if (nneighbors[pj].count(pi) == 0) {
222 nneighbors[pj].insert(pi);
224 if (nneighbors[pj].count(pk) == 0) {
226 nneighbors[pj].insert(pk);
231 nneighbors_ =
new unsigned int[c_nneighbors];
232 unsigned int* p_nneighbors = nneighbors_;
233 for (
unsigned int i = 0; i < num_points; ++i) {
235 if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)())
236 throw std::logic_error (
"Too many neighbors.");
237 n.
count_ = (
unsigned char)nneighbors[i].size();
239 p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
241 assert (p_nneighbors == nneighbors_ + c_nneighbors);
FCL_REAL computeVolume() const
compute the volume
Definition: convex.hxx:165
bool own_storage_
Definition: geometric_shapes.h:445
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Set the points of the convex shape.
Main namespace.
Definition: AABB.h:43
Definition: geometric_shapes.h:401
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
void fillNeighbors()
Definition: convex.hxx:198
Vec3f computeCOM() const
compute center of mass
Definition: convex.hxx:131
unsigned int num_polygons
Definition: convex.h:81
~Convex()
Definition: convex.hxx:72
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:404
unsigned char count_
Definition: geometric_shapes.h:403
void initialize(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
Convex()
Construct an uninitialized convex object.
Definition: convex.h:57
Base for convex polytope.
Definition: geometric_shapes.h:355
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_, PolygonT *polygons_, unsigned int num_polygons_)
Set the current Convex from a list of points and polygons.
Definition: convex.hxx:78
Matrix3f computeMomentofInertia() const
based on http://number-none.com/blow/inertia/bb_inertia.doc
Definition: convex.hxx:92
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Convex polytope.
Definition: convex.h:53