00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "fcl/BVH/BV_splitter.h"
00038
00039 namespace fcl
00040 {
00041
00042
00043 template<typename BV>
00044 void computeSplitVector(const BV& bv, Vec3f& split_vector)
00045 {
00046 split_vector = bv.axis[0];
00047 }
00048
00049 template<>
00050 void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector)
00051 {
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080 split_vector = bv.obb.axis[0];
00081 }
00082
00083 template<>
00084 void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3f& split_vector)
00085 {
00086 split_vector = bv.obb.axis[0];
00087 }
00088
00089 template<typename BV>
00090 void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value)
00091 {
00092 Vec3f center = bv.center();
00093 split_value = center[0];
00094 }
00095
00096 template<typename BV>
00097 void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value)
00098 {
00099 FCL_REAL sum = 0.0;
00100 if(type == BVH_MODEL_TRIANGLES)
00101 {
00102 FCL_REAL c[3] = {0.0, 0.0, 0.0};
00103
00104 for(int i = 0; i < num_primitives; ++i)
00105 {
00106 const Triangle& t = triangles[primitive_indices[i]];
00107 const Vec3f& p1 = vertices[t[0]];
00108 const Vec3f& p2 = vertices[t[1]];
00109 const Vec3f& p3 = vertices[t[2]];
00110
00111 c[0] += (p1[0] + p2[0] + p3[0]);
00112 c[1] += (p1[1] + p2[1] + p3[1]);
00113 c[2] += (p1[2] + p2[2] + p3[2]);
00114 }
00115 split_value = (c[0] * split_vector[0] + c[1] * split_vector[1] + c[2] * split_vector[2]) / (3 * num_primitives);
00116 }
00117 else if(type == BVH_MODEL_POINTCLOUD)
00118 {
00119 for(int i = 0; i < num_primitives; ++i)
00120 {
00121 const Vec3f& p = vertices[primitive_indices[i]];
00122 Vec3f v(p[0], p[1], p[2]);
00123 sum += v.dot(split_vector);
00124 }
00125
00126 split_value = sum / num_primitives;
00127 }
00128 }
00129
00130 template<typename BV>
00131 void computeSplitValue_median(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value)
00132 {
00133 std::vector<FCL_REAL> proj(num_primitives);
00134
00135 if(type == BVH_MODEL_TRIANGLES)
00136 {
00137 for(int i = 0; i < num_primitives; ++i)
00138 {
00139 const Triangle& t = triangles[primitive_indices[i]];
00140 const Vec3f& p1 = vertices[t[0]];
00141 const Vec3f& p2 = vertices[t[1]];
00142 const Vec3f& p3 = vertices[t[2]];
00143 Vec3f centroid3(p1[0] + p2[0] + p3[0],
00144 p1[1] + p2[1] + p3[1],
00145 p1[2] + p2[2] + p3[2]);
00146
00147 proj[i] = centroid3.dot(split_vector) / 3;
00148 }
00149 }
00150 else if(type == BVH_MODEL_POINTCLOUD)
00151 {
00152 for(int i = 0; i < num_primitives; ++i)
00153 {
00154 const Vec3f& p = vertices[primitive_indices[i]];
00155 Vec3f v(p[0], p[1], p[2]);
00156 proj[i] = v.dot(split_vector);
00157 }
00158 }
00159
00160 std::sort(proj.begin(), proj.end());
00161
00162 if(num_primitives % 2 == 1)
00163 {
00164 split_value = proj[(num_primitives - 1) / 2];
00165 }
00166 else
00167 {
00168 split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
00169 }
00170 }
00171
00172 template<>
00173 void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
00174 {
00175 computeSplitVector<OBB>(bv, split_vector);
00176 computeSplitValue_bvcenter<OBB>(bv, split_value);
00177 }
00178
00179 template<>
00180 void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
00181 {
00182 computeSplitVector<OBB>(bv, split_vector);
00183 computeSplitValue_mean<OBB>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00184 }
00185
00186 template<>
00187 void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
00188 {
00189 computeSplitVector<OBB>(bv, split_vector);
00190 computeSplitValue_median<OBB>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00191 }
00192
00193 template<>
00194 void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
00195 {
00196 computeSplitVector<RSS>(bv, split_vector);
00197 computeSplitValue_bvcenter<RSS>(bv, split_value);
00198 }
00199
00200 template<>
00201 void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
00202 {
00203 computeSplitVector<RSS>(bv, split_vector);
00204 computeSplitValue_mean<RSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00205 }
00206
00207 template<>
00208 void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
00209 {
00210 computeSplitVector<RSS>(bv, split_vector);
00211 computeSplitValue_median<RSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00212 }
00213
00214 template<>
00215 void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
00216 {
00217 computeSplitVector<kIOS>(bv, split_vector);
00218 computeSplitValue_bvcenter<kIOS>(bv, split_value);
00219 }
00220
00221 template<>
00222 void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
00223 {
00224 computeSplitVector<kIOS>(bv, split_vector);
00225 computeSplitValue_mean<kIOS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00226 }
00227
00228 template<>
00229 void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
00230 {
00231 computeSplitVector<kIOS>(bv, split_vector);
00232 computeSplitValue_median<kIOS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00233 }
00234
00235 template<>
00236 void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
00237 {
00238 computeSplitVector<OBBRSS>(bv, split_vector);
00239 computeSplitValue_bvcenter<OBBRSS>(bv, split_value);
00240 }
00241
00242 template<>
00243 void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
00244 {
00245 computeSplitVector<OBBRSS>(bv, split_vector);
00246 computeSplitValue_mean<OBBRSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00247 }
00248
00249 template<>
00250 void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives)
00251 {
00252 computeSplitVector<OBBRSS>(bv, split_vector);
00253 computeSplitValue_median<OBBRSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
00254 }
00255
00256
00257 template<>
00258 bool BVSplitter<OBB>::apply(const Vec3f& q) const
00259 {
00260 return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
00261 }
00262
00263 template<>
00264 bool BVSplitter<RSS>::apply(const Vec3f& q) const
00265 {
00266 return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
00267 }
00268
00269 template<>
00270 bool BVSplitter<kIOS>::apply(const Vec3f& q) const
00271 {
00272 return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
00273 }
00274
00275 template<>
00276 bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const
00277 {
00278 return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
00279 }
00280
00281
00282 template class BVSplitter<RSS>;
00283 template class BVSplitter<OBBRSS>;
00284 template class BVSplitter<OBB>;
00285 template class BVSplitter<kIOS>;
00286
00287 }