BV_splitter.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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     switch(bv.num_spheres)
00054     {
00055     case 1:
00056     split_vector = Vec3f(1, 0, 0);
00057     break;
00058     case 3:
00059     {
00060     Vec3f v[3];
00061     v[0] = bv.spheres[1].o - bv.spheres[0].o;
00062     v[0].normalize();
00063     generateCoordinateSystem(v[0], v[1], v[2]);
00064     split_vector = v[1];
00065     }
00066     break;
00067     case 5:
00068     {
00069     Vec3f v[2];
00070     v[0] = bv.spheres[1].o - bv.spheres[0].o;
00071     v[1] = bv.spheres[3].o - bv.spheres[0].o;
00072     split_vector = v[0].cross(v[1]);
00073     split_vector.normalize();
00074     }
00075     break;
00076     default:
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30