38 #ifndef HPP_FCL_SHAPE_CONVEX_HXX
39 #define HPP_FCL_SHAPE_CONVEX_HXX
49 template <
typename PolygonT>
51 PolygonT* polygons_,
int num_polygons_) :
54 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>
80 typedef typename PolygonT::size_type size_type;
81 typedef typename PolygonT::index_type index_type;
86 C_canonical << 1/60.0, 1/120.0, 1/120.0,
87 1/120.0, 1/60.0, 1/120.0,
88 1/120.0, 1/120.0, 1/60.0;
90 for(
int i = 0; i < num_polygons; ++i)
92 const PolygonT& polygon (polygons[i]);
95 Vec3f plane_center(0,0,0);
96 for(size_type j = 0; j < polygon.size(); ++j)
97 plane_center += points[polygon[j]];
98 plane_center /= polygon.size();
101 const Vec3f& v3 = plane_center;
102 for(size_type j = 0; j < polygon.size(); ++j)
104 index_type e_first = polygon[j];
105 index_type e_second = polygon[(j+1)%polygon.size()];
106 const Vec3f& v1 = points[e_first];
107 const Vec3f& v2 = points[e_second];
108 Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
109 C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
113 return C.trace() * Matrix3f::Identity() - C;
116 template <
typename PolygonT>
119 typedef typename PolygonT::size_type size_type;
120 typedef typename PolygonT::index_type index_type;
124 for(
int i = 0; i < num_polygons; ++i)
126 const PolygonT& polygon (polygons[i]);
128 Vec3f plane_center(0,0,0);
129 for(size_type j = 0; j < polygon.size(); ++j)
130 plane_center += points[polygon[j]];
131 plane_center /= polygon.size();
134 const Vec3f& v3 = plane_center;
135 for(size_type j = 0; j < polygon.size(); ++j)
137 index_type e_first = polygon[j];
138 index_type e_second = polygon[(j+1)%polygon.size()];
139 const Vec3f& v1 = points[e_first];
140 const Vec3f& v2 = points[e_second];
141 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
143 com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
147 return com / (vol * 4);
150 template <
typename PolygonT>
153 typedef typename PolygonT::size_type size_type;
154 typedef typename PolygonT::index_type index_type;
157 for(
int i = 0; i < num_polygons; ++i)
159 const PolygonT& polygon (polygons[i]);
162 Vec3f plane_center(0,0,0);
163 for(size_type j = 0; j < polygon.size(); ++j)
164 plane_center += points[polygon[j]];
165 plane_center /= polygon.size();
168 const Vec3f& v3 = plane_center;
169 for(size_type j = 0; j < polygon.size(); ++j)
171 index_type e_first = polygon[j];
172 index_type e_second = polygon[(j+1)%polygon.size()];
173 const Vec3f& v1 = points[e_first];
174 const Vec3f& v2 = points[e_second];
175 FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
183 template <
typename PolygonT>
188 typedef typename PolygonT::size_type size_type;
189 typedef typename PolygonT::index_type index_type;
190 std::vector<std::set<index_type> > nneighbors (num_points);
191 unsigned int c_nneighbors = 0;
193 for (
int l = 0; l < num_polygons; ++l)
195 const PolygonT& polygon (polygons[l]);
196 size_type n = polygon.size();
198 for(size_type j = 0; j < polygon.size(); ++j)
200 size_type i = (j==0 ) ? n-1 : j-1;
201 size_type k = (j==n-1) ? 0 : j+1;
202 index_type pi = polygon[i],
206 if (nneighbors[pj].count(pi) == 0) {
208 nneighbors[pj].insert(pi);
210 if (nneighbors[pj].count(pk) == 0) {
212 nneighbors[pj].insert(pk);
217 nneighbors_ =
new unsigned int[c_nneighbors];
218 unsigned int* p_nneighbors = nneighbors_;
219 for (
int i = 0; i < num_points; ++i) {
221 if (nneighbors[i].size() >= std::numeric_limits<unsigned char>::max())
222 throw std::logic_error (
"Too many neighbors.");
223 n.
count_ = (
unsigned char)nneighbors[i].size();
225 p_nneighbors = std::copy (nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors);
227 assert (p_nneighbors == nneighbors_ + c_nneighbors);