geometric_shape_to_BVH_model.h
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 
00038 #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H
00039 #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H
00040 
00041 #include "fcl/shape/geometric_shapes.h"
00042 #include "fcl/BVH/BVH_model.h"
00043 #include <boost/math/constants/constants.hpp>
00044 
00045 namespace fcl
00046 {
00047 
00049 template<typename BV>
00050 void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
00051 {
00052   double a = shape.side[0];
00053   double b = shape.side[1];
00054   double c = shape.side[2];
00055   std::vector<Vec3f> points(8);
00056   std::vector<Triangle> tri_indices(12);
00057   points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c);
00058   points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c);
00059   points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c);
00060   points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c);
00061   points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c);
00062   points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c);
00063   points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c);
00064   points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c);
00065 
00066   tri_indices[0].set(0, 4, 1);
00067   tri_indices[1].set(1, 4, 5);
00068   tri_indices[2].set(2, 6, 3);
00069   tri_indices[3].set(3, 6, 7);
00070   tri_indices[4].set(3, 0, 2);
00071   tri_indices[5].set(2, 0, 1);
00072   tri_indices[6].set(6, 5, 7);
00073   tri_indices[7].set(7, 5, 4);
00074   tri_indices[8].set(1, 5, 2);
00075   tri_indices[9].set(2, 5, 6);
00076   tri_indices[10].set(3, 7, 0);
00077   tri_indices[11].set(0, 7, 4);
00078 
00079   for(unsigned int i = 0; i < points.size(); ++i)
00080   {
00081     points[i] = pose.transform(points[i]);
00082   }
00083 
00084   model.beginModel();
00085   model.addSubModel(points, tri_indices);
00086   model.endModel();
00087   model.computeLocalAABB();
00088 }
00089 
00091 template<typename BV>
00092 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring)
00093 {
00094   std::vector<Vec3f> points;
00095   std::vector<Triangle> tri_indices;
00096 
00097   double r = shape.radius;
00098   double phi, phid;
00099   const double pi = boost::math::constants::pi<double>();
00100   phid = pi * 2 / seg;
00101   phi = 0;
00102 
00103   double theta, thetad;
00104   thetad = pi / (ring + 1);
00105   theta = 0;
00106 
00107   for(unsigned int i = 0; i < ring; ++i)
00108   {
00109     double theta_ = theta + thetad * (i + 1);
00110     for(unsigned int j = 0; j < seg; ++j)
00111     {
00112       points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
00113     }
00114   }
00115   points.push_back(Vec3f(0, 0, r));
00116   points.push_back(Vec3f(0, 0, -r));
00117 
00118   for(unsigned int i = 0; i < ring - 1; ++i)
00119   {
00120     for(unsigned int j = 0; j < seg; ++j)
00121     {
00122        unsigned int a, b, c, d;
00123        a = i * seg + j;
00124        b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
00125        c = (i + 1) * seg + j;
00126        d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
00127        tri_indices.push_back(Triangle(a, c, b));
00128        tri_indices.push_back(Triangle(b, c, d));
00129     }
00130   }
00131 
00132   for(unsigned int j = 0; j < seg; ++j)
00133   {
00134     unsigned int a, b;
00135     a = j;
00136     b = (j == seg - 1) ? 0 : (j + 1);
00137     tri_indices.push_back(Triangle(ring * seg, a, b));
00138 
00139     a = (ring - 1) * seg + j;
00140     b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
00141     tri_indices.push_back(Triangle(a, ring * seg + 1, b));
00142   }
00143 
00144   for(unsigned int i = 0; i < points.size(); ++i)
00145   {
00146     points[i] = pose.transform(points[i]);
00147   }
00148 
00149   model.beginModel();
00150   model.addSubModel(points, tri_indices);
00151   model.endModel();
00152   model.computeLocalAABB();
00153 }
00154 
00158 template<typename BV>
00159 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
00160 {
00161   double r = shape.radius;
00162   double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
00163   unsigned int ring = ceil(n_low_bound);
00164   unsigned int seg = ceil(n_low_bound);
00165 
00166   generateBVHModel(model, shape, pose, seg, ring);  
00167 }
00168 
00169 
00171 template<typename BV>
00172 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
00173 {
00174   std::vector<Vec3f> points;
00175   std::vector<Triangle> tri_indices;
00176 
00177   double r = shape.radius;
00178   double h = shape.lz;
00179   double phi, phid;
00180   const double pi = boost::math::constants::pi<double>();
00181   phid = pi * 2 / tot;
00182   phi = 0;
00183 
00184   double hd = h / h_num;
00185 
00186   for(unsigned int i = 0; i < tot; ++i)
00187     points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
00188 
00189   for(unsigned int i = 0; i < h_num - 1; ++i)
00190   {
00191     for(unsigned int j = 0; j < tot; ++j)
00192     {
00193       points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
00194     }
00195   }
00196 
00197   for(unsigned int i = 0; i < tot; ++i)
00198     points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
00199 
00200   points.push_back(Vec3f(0, 0, h / 2));
00201   points.push_back(Vec3f(0, 0, -h / 2));
00202 
00203   for(unsigned int i = 0; i < tot; ++i)
00204   {
00205     Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
00206     tri_indices.push_back(tmp);
00207   }
00208 
00209   for(unsigned int i = 0; i < tot; ++i)
00210   {
00211     Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
00212     tri_indices.push_back(tmp);
00213   }
00214 
00215   for(unsigned int i = 0; i < h_num; ++i)
00216   {
00217     for(unsigned int j = 0; j < tot; ++j)
00218     {
00219       int a, b, c, d;
00220       a = j;
00221       b = (j == tot - 1) ? 0 : (j + 1);
00222       c = j + tot;
00223       d = (j == tot - 1) ? tot : (j + 1 + tot);
00224 
00225       int start = i * tot;
00226       tri_indices.push_back(Triangle(start + b, start + a, start + c));
00227       tri_indices.push_back(Triangle(start + b, start + c, start + d));
00228     }
00229   }
00230 
00231   for(unsigned int i = 0; i < points.size(); ++i)
00232   {
00233     points[i] = pose.transform(points[i]);
00234   }
00235 
00236   model.beginModel();
00237   model.addSubModel(points, tri_indices);
00238   model.endModel();
00239   model.computeLocalAABB();
00240 }
00241 
00245 template<typename BV>
00246 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
00247 {
00248   double r = shape.radius;
00249   double h = shape.lz;
00250 
00251   const double pi = boost::math::constants::pi<double>();
00252   unsigned int tot = tot_for_unit_cylinder * r;
00253   double phid = pi * 2 / tot;
00254 
00255   double circle_edge = phid * r;
00256   unsigned int h_num = ceil(h / circle_edge);
00257   
00258   generateBVHModel(model, shape, pose, tot, h_num);
00259 }
00260 
00261 
00263 template<typename BV>
00264 void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
00265 {
00266   std::vector<Vec3f> points;
00267   std::vector<Triangle> tri_indices;
00268 
00269   double r = shape.radius;
00270   double h = shape.lz;
00271 
00272   double phi, phid;
00273   const double pi = boost::math::constants::pi<double>();
00274   phid = pi * 2 / tot;
00275   phi = 0;
00276 
00277   double hd = h / h_num;
00278 
00279   for(unsigned int i = 0; i < h_num - 1; ++i)
00280   {
00281     for(unsigned int j = 0; j < tot; ++j)
00282     {
00283       points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
00284     }
00285   }
00286 
00287   for(unsigned int i = 0; i < tot; ++i)
00288     points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
00289 
00290   points.push_back(Vec3f(0, 0, h / 2));
00291   points.push_back(Vec3f(0, 0, -h / 2));
00292 
00293   for(unsigned int i = 0; i < tot; ++i)
00294   {
00295     Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
00296     tri_indices.push_back(tmp);
00297   }
00298 
00299   for(unsigned int i = 0; i < tot; ++i)
00300   {
00301     Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i);
00302     tri_indices.push_back(tmp);
00303   }
00304 
00305   for(unsigned int i = 0; i < h_num - 1; ++i)
00306   {
00307     for(unsigned int j = 0; j < tot; ++j)
00308     {
00309       int a, b, c, d;
00310       a = j;
00311       b = (j == tot - 1) ? 0 : (j + 1);
00312       c = j + tot;
00313       d = (j == tot - 1) ? tot : (j + 1 + tot);
00314 
00315       int start = i * tot;
00316       tri_indices.push_back(Triangle(start + b, start + a, start + c));
00317       tri_indices.push_back(Triangle(start + b, start + c, start + d));
00318     }
00319   }
00320 
00321   for(unsigned int i = 0; i < points.size(); ++i)
00322   {
00323     points[i] = pose.transform(points[i]);
00324   }
00325 
00326   model.beginModel();
00327   model.addSubModel(points, tri_indices);
00328   model.endModel();
00329   model.computeLocalAABB();
00330 }
00331 
00335 template<typename BV>
00336 void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
00337 {
00338   double r = shape.radius;
00339   double h = shape.lz;
00340 
00341   const double pi = boost::math::constants::pi<double>();
00342   unsigned int tot = tot_for_unit_cone * r;
00343   double phid = pi * 2 / tot;
00344 
00345   double circle_edge = phid * r;
00346   unsigned int h_num = ceil(h / circle_edge);
00347 
00348   generateBVHModel(model, shape, pose, tot, h_num);
00349 }
00350 
00351 }
00352 
00353 #endif
 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