BVH_utility.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 
00038 #include "fcl/BVH/BVH_utility.h"
00039 
00040 namespace fcl
00041 {
00042 
00043 void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
00044 {
00045   for(int i = 0; i < model.getNumBVs(); ++i)
00046   {
00047     BVNode<OBB>& bvnode = model.getBV(i);
00048 
00049     Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
00050 
00051     for(int j = 0; j < bvnode.num_primitives; ++j)
00052     {
00053       int v_id = bvnode.first_primitive + j;
00054       const Variance3f& uc = ucs[v_id];
00055 
00056       Vec3f&v = model.vertices[bvnode.first_primitive + j];
00057 
00058       for(int k = 0; k < 3; ++k)
00059       {
00060         vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
00061         vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
00062       }
00063     }
00064 
00065     OBB bv;
00066     fit(vs, bvnode.num_primitives * 6, bv);
00067 
00068     delete [] vs;
00069 
00070     bvnode.bv = bv;
00071   }
00072 }
00073 
00074 void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
00075 {
00076   for(int i = 0; i < model.getNumBVs(); ++i)
00077   {
00078     BVNode<RSS>& bvnode = model.getBV(i);
00079 
00080     Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
00081 
00082     for(int j = 0; j < bvnode.num_primitives; ++j)
00083     {
00084       int v_id = bvnode.first_primitive + j;
00085       const Variance3f& uc = ucs[v_id];
00086 
00087       Vec3f&v = model.vertices[bvnode.first_primitive + j];
00088 
00089       for(int k = 0; k < 3; ++k)
00090       {
00091         vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
00092         vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
00093       }
00094     }
00095 
00096     RSS bv;
00097     fit(vs, bvnode.num_primitives * 6, bv);
00098 
00099     delete [] vs;
00100 
00101     bvnode.bv = bv;
00102   }
00103 }
00104 
00105 
00106 void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
00107 {
00108   Vec3f S1;
00109   Vec3f S2[3];
00110 
00111   if(ts)
00112   {
00113     for(int i = 0; i < n; ++i)
00114     {
00115       const Triangle& t = (indices) ? ts[indices[i]] : ts[i];
00116 
00117       const Vec3f& p1 = ps[t[0]];
00118       const Vec3f& p2 = ps[t[1]];
00119       const Vec3f& p3 = ps[t[2]];
00120 
00121       S1[0] += (p1[0] + p2[0] + p3[0]);
00122       S1[1] += (p1[1] + p2[1] + p3[1]);
00123       S1[2] += (p1[2] + p2[2] + p3[2]);
00124       S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
00125       S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
00126       S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
00127       S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
00128       S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
00129       S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
00130 
00131       if(ps2)
00132       {
00133         const Vec3f& p1 = ps2[t[0]];
00134         const Vec3f& p2 = ps2[t[1]];
00135         const Vec3f& p3 = ps2[t[2]];
00136 
00137         S1[0] += (p1[0] + p2[0] + p3[0]);
00138         S1[1] += (p1[1] + p2[1] + p3[1]);
00139         S1[2] += (p1[2] + p2[2] + p3[2]);
00140 
00141         S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
00142         S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
00143         S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
00144         S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
00145         S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
00146         S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
00147       }
00148     }
00149   }
00150   else
00151   {
00152     for(int i = 0; i < n; ++i)
00153     {
00154       const Vec3f& p = (indices) ? ps[indices[i]] : ps[i];
00155       S1 += p;
00156       S2[0][0] += (p[0] * p[0]);
00157       S2[1][1] += (p[1] * p[1]);
00158       S2[2][2] += (p[2] * p[2]);
00159       S2[0][1] += (p[0] * p[1]);
00160       S2[0][2] += (p[0] * p[2]);
00161       S2[1][2] += (p[1] * p[2]);
00162 
00163       if(ps2) // another frame
00164       {
00165         const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i];
00166         S1 += p;
00167         S2[0][0] += (p[0] * p[0]);
00168         S2[1][1] += (p[1] * p[1]);
00169         S2[2][2] += (p[2] * p[2]);
00170         S2[0][1] += (p[0] * p[1]);
00171         S2[0][2] += (p[0] * p[2]);
00172         S2[1][2] += (p[1] * p[2]);
00173       }
00174     }
00175   }
00176 
00177   int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
00178 
00179   M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points;
00180   M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points;
00181   M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points;
00182   M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points;
00183   M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points;
00184   M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points;
00185   M(1, 0) = M(0, 1);
00186   M(2, 0) = M(0, 2);
00187   M(2, 1) = M(1, 2);
00188 }
00189 
00190 
00194 void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r)
00195 {
00196   bool indirect_index = true;
00197   if(!indices) indirect_index = false;
00198 
00199   int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
00200 
00201   FCL_REAL (*P)[3] = new FCL_REAL[size_P][3];
00202 
00203   int P_id = 0;
00204   
00205   if(ts)
00206   {
00207     for(int i = 0; i < n; ++i)
00208     {
00209       int index = indirect_index ? indices[i] : i;
00210       const Triangle& t = ts[index];
00211 
00212       for(int j = 0; j < 3; ++j)
00213       {
00214         int point_id = t[j];
00215         const Vec3f& p = ps[point_id];
00216         Vec3f v(p[0], p[1], p[2]);
00217         P[P_id][0] = axis[0].dot(v);
00218         P[P_id][1] = axis[1].dot(v);
00219         P[P_id][2] = axis[2].dot(v);
00220         P_id++;
00221       }
00222 
00223       if(ps2)
00224       {
00225         for(int j = 0; j < 3; ++j)
00226         {
00227           int point_id = t[j];
00228           const Vec3f& p = ps2[point_id];
00229           Vec3f v(p[0], p[1], p[2]);
00230           P[P_id][0] = axis[0].dot(v);
00231           P[P_id][1] = axis[0].dot(v);
00232           P[P_id][2] = axis[1].dot(v);
00233           P_id++;
00234         }
00235       }
00236     }
00237   }
00238   else
00239   {
00240     for(int i = 0; i < n; ++i)
00241     {
00242       int index = indirect_index ? indices[i] : i;
00243 
00244       const Vec3f& p = ps[index];
00245       Vec3f v(p[0], p[1], p[2]);
00246       P[P_id][0] = axis[0].dot(v);
00247       P[P_id][1] = axis[1].dot(v);
00248       P[P_id][2] = axis[2].dot(v);
00249       P_id++;
00250 
00251       if(ps2)
00252       {
00253         const Vec3f& v = ps2[index];
00254         P[P_id][0] = axis[0].dot(v);
00255         P[P_id][1] = axis[1].dot(v);
00256         P[P_id][2] = axis[2].dot(v);
00257         P_id++;
00258       }
00259     }
00260   }
00261     
00262   FCL_REAL minx, maxx, miny, maxy, minz, maxz;
00263 
00264   FCL_REAL cz, radsqr;
00265 
00266   minz = maxz = P[0][2];
00267 
00268   for(int i = 1; i < size_P; ++i)
00269   {
00270     FCL_REAL z_value = P[i][2];
00271     if(z_value < minz) minz = z_value;
00272     else if(z_value > maxz) maxz = z_value;
00273   }
00274 
00275   r = (FCL_REAL)0.5 * (maxz - minz);
00276   radsqr = r * r;
00277   cz = (FCL_REAL)0.5 * (maxz + minz);
00278 
00279   // compute an initial length of rectangle along x direction
00280 
00281   // find minx and maxx as starting points
00282 
00283   int minindex, maxindex;
00284   minindex = maxindex = 0;
00285   FCL_REAL mintmp, maxtmp;
00286   mintmp = maxtmp = P[0][0];
00287 
00288   for(int i = 1; i < size_P; ++i)
00289   {
00290     FCL_REAL x_value = P[i][0];
00291     if(x_value < mintmp)
00292     {
00293       minindex = i;
00294       mintmp = x_value;
00295     }
00296     else if(x_value > maxtmp)
00297     {
00298       maxindex = i;
00299       maxtmp = x_value;
00300     }
00301   }
00302 
00303   FCL_REAL x, dz;
00304   dz = P[minindex][2] - cz;
00305   minx = P[minindex][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00306   dz = P[maxindex][2] - cz;
00307   maxx = P[maxindex][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00308 
00309 
00310   // grow minx
00311 
00312   for(int i = 0; i < size_P; ++i)
00313   {
00314     if(P[i][0] < minx)
00315     {
00316       dz = P[i][2] - cz;
00317       x = P[i][0] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00318       if(x < minx) minx = x;
00319     }
00320   }
00321 
00322   // grow maxx
00323 
00324   for(int i = 0; i < size_P; ++i)
00325   {
00326     if(P[i][0] > maxx)
00327     {
00328       dz = P[i][2] - cz;
00329       x = P[i][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00330       if(x > maxx) maxx = x;
00331     }
00332   }
00333 
00334   // compute an initial length of rectangle along y direction
00335 
00336   // find miny and maxy as starting points
00337 
00338   minindex = maxindex = 0;
00339   mintmp = maxtmp = P[0][1];
00340   for(int i = 1; i < size_P; ++i)
00341   {
00342     FCL_REAL y_value = P[i][1];
00343     if(y_value < mintmp)
00344     {
00345       minindex = i;
00346       mintmp = y_value;
00347     }
00348     else if(y_value > maxtmp)
00349     {
00350       maxindex = i;
00351       maxtmp = y_value;
00352     }
00353   }
00354 
00355   FCL_REAL y;
00356   dz = P[minindex][2] - cz;
00357   miny = P[minindex][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00358   dz = P[maxindex][2] - cz;
00359   maxy = P[maxindex][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00360 
00361   // grow miny
00362 
00363   for(int i = 0; i < size_P; ++i)
00364   {
00365     if(P[i][1] < miny)
00366     {
00367       dz = P[i][2] - cz;
00368       y = P[i][1] + std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00369       if(y < miny) miny = y;
00370     }
00371   }
00372 
00373   // grow maxy
00374 
00375   for(int i = 0; i < size_P; ++i)
00376   {
00377     if(P[i][1] > maxy)
00378     {
00379       dz = P[i][2] - cz;
00380       y = P[i][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
00381       if(y > maxy) maxy = y;
00382     }
00383   }
00384 
00385   // corners may have some points which are not covered - grow lengths if necessary
00386   // quite conservative (can be improved)
00387   FCL_REAL dx, dy, u, t;
00388   FCL_REAL a = std::sqrt((FCL_REAL)0.5);
00389   for(int i = 0; i < size_P; ++i)
00390   {
00391     if(P[i][0] > maxx)
00392     {
00393       if(P[i][1] > maxy)
00394       {
00395         dx = P[i][0] - maxx;
00396         dy = P[i][1] - maxy;
00397         u = dx * a + dy * a;
00398         t = (a*u - dx)*(a*u - dx) +
00399             (a*u - dy)*(a*u - dy) +
00400             (cz - P[i][2])*(cz - P[i][2]);
00401         u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
00402         if(u > 0)
00403         {
00404           maxx += u*a;
00405           maxy += u*a;
00406         }
00407       }
00408       else if(P[i][1] < miny)
00409       {
00410         dx = P[i][0] - maxx;
00411         dy = P[i][1] - miny;
00412         u = dx * a - dy * a;
00413         t = (a*u - dx)*(a*u - dx) +
00414             (-a*u - dy)*(-a*u - dy) +
00415             (cz - P[i][2])*(cz - P[i][2]);
00416         u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
00417         if(u > 0)
00418         {
00419           maxx += u*a;
00420           miny -= u*a;
00421         }
00422       }
00423     }
00424     else if(P[i][0] < minx)
00425     {
00426       if(P[i][1] > maxy)
00427       {
00428         dx = P[i][0] - minx;
00429         dy = P[i][1] - maxy;
00430         u = dy * a - dx * a;
00431         t = (-a*u - dx)*(-a*u - dx) +
00432             (a*u - dy)*(a*u - dy) +
00433             (cz - P[i][2])*(cz - P[i][2]);
00434         u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
00435         if(u > 0)
00436         {
00437           minx -= u*a;
00438           maxy += u*a;
00439         }
00440       }
00441       else if(P[i][1] < miny)
00442       {
00443         dx = P[i][0] - minx;
00444         dy = P[i][1] - miny;
00445         u = -dx * a - dy * a;
00446         t = (-a*u - dx)*(-a*u - dx) +
00447             (-a*u - dy)*(-a*u - dy) +
00448             (cz - P[i][2])*(cz - P[i][2]);
00449         u = u - std::sqrt(std::max<FCL_REAL>(radsqr - t, 0));
00450         if (u > 0)
00451         {
00452           minx -= u*a;
00453           miny -= u*a;
00454         }
00455       }
00456     }
00457   }
00458 
00459   origin = axis[0] * minx + axis[1] * miny + axis[2] * cz;
00460 
00461   l[0] = maxx - minx;
00462   if(l[0] < 0) l[0] = 0;
00463   l[1] = maxy - miny;
00464   if(l[1] < 0) l[1] = 0;
00465 
00466   delete [] P;
00467 
00468 }
00469 
00470 
00474 static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
00475 {
00476   bool indirect_index = true;
00477   if(!indices) indirect_index = false;
00478 
00479   FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
00480 
00481   FCL_REAL min_coord[3] = {real_max, real_max, real_max};
00482   FCL_REAL max_coord[3] = {-real_max, -real_max, -real_max};
00483 
00484   for(int i = 0; i < n; ++i)
00485   {
00486     int index = indirect_index ? indices[i] : i;
00487 
00488     const Vec3f& p = ps[index];
00489     Vec3f v(p[0], p[1], p[2]);
00490     FCL_REAL proj[3];
00491     proj[0] = axis[0].dot(v);
00492     proj[1] = axis[1].dot(v);
00493     proj[2] = axis[2].dot(v);
00494 
00495     for(int j = 0; j < 3; ++j)
00496     {
00497       if(proj[j] > max_coord[j]) max_coord[j] = proj[j];
00498       if(proj[j] < min_coord[j]) min_coord[j] = proj[j];
00499     }
00500 
00501     if(ps2)
00502     {
00503       const Vec3f& v = ps2[index];
00504       proj[0] = axis[0].dot(v);
00505       proj[1] = axis[1].dot(v);
00506       proj[2] = axis[2].dot(v);
00507 
00508       for(int j = 0; j < 3; ++j)
00509       {
00510         if(proj[j] > max_coord[j]) max_coord[j] = proj[j];
00511         if(proj[j] < min_coord[j]) min_coord[j] = proj[j];
00512       }
00513     }
00514   }
00515 
00516   Vec3f o((max_coord[0] + min_coord[0]) / 2,
00517           (max_coord[1] + min_coord[1]) / 2,
00518           (max_coord[2] + min_coord[2]) / 2);
00519 
00520   center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
00521 
00522   extent.setValue((max_coord[0] - min_coord[0]) / 2,
00523                   (max_coord[1] - min_coord[1]) / 2,
00524                   (max_coord[2] - min_coord[2]) / 2);
00525 
00526 }
00527 
00528 
00532 static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
00533 {
00534   bool indirect_index = true;
00535   if(!indices) indirect_index = false;
00536 
00537   FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
00538 
00539   FCL_REAL min_coord[3] = {real_max, real_max, real_max};
00540   FCL_REAL max_coord[3] = {-real_max, -real_max, -real_max};
00541 
00542   for(int i = 0; i < n; ++i)
00543   {
00544     unsigned int index = indirect_index? indices[i] : i;
00545     const Triangle& t = ts[index];
00546 
00547     for(int j = 0; j < 3; ++j)
00548     {
00549       int point_id = t[j];
00550       const Vec3f& p = ps[point_id];
00551       Vec3f v(p[0], p[1], p[2]);
00552       FCL_REAL proj[3];
00553       proj[0] = axis[0].dot(v);
00554       proj[1] = axis[1].dot(v);
00555       proj[2] = axis[2].dot(v);
00556 
00557       for(int k = 0; k < 3; ++k)
00558       {
00559         if(proj[k] > max_coord[k]) max_coord[k] = proj[k];
00560         if(proj[k] < min_coord[k]) min_coord[k] = proj[k];
00561       }
00562     }
00563 
00564     if(ps2)
00565     {
00566       for(int j = 0; j < 3; ++j)
00567       {
00568         int point_id = t[j];
00569         const Vec3f& p = ps2[point_id];
00570         Vec3f v(p[0], p[1], p[2]);
00571         FCL_REAL proj[3];
00572         proj[0] = axis[0].dot(v);
00573         proj[1] = axis[1].dot(v);
00574         proj[2] = axis[2].dot(v);
00575 
00576         for(int k = 0; k < 3; ++k)
00577         {
00578           if(proj[k] > max_coord[k]) max_coord[k] = proj[k];
00579           if(proj[k] < min_coord[k]) min_coord[k] = proj[k];
00580         }
00581       }
00582     }
00583   }
00584 
00585   Vec3f o((max_coord[0] + min_coord[0]) / 2,
00586           (max_coord[1] + min_coord[1]) / 2,
00587           (max_coord[2] + min_coord[2]) / 2);
00588 
00589   center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2];
00590 
00591   extent.setValue((max_coord[0] - min_coord[0]) / 2,
00592                   (max_coord[1] - min_coord[1]) / 2,
00593                   (max_coord[2] - min_coord[2]) / 2);
00594 
00595 }
00596 
00597 void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent)
00598 {
00599   if(ts)
00600     getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axis, center, extent);
00601   else
00602     getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent);
00603 }
00604 
00605 void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius)
00606 {
00607   Vec3f e1 = a - c;
00608   Vec3f e2 = b - c;
00609   FCL_REAL e1_len2 = e1.sqrLength();
00610   FCL_REAL e2_len2 = e2.sqrLength();
00611   Vec3f e3 = e1.cross(e2);
00612   FCL_REAL e3_len2 = e3.sqrLength();
00613   radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
00614   radius = std::sqrt(radius) * 0.5;
00615 
00616   center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
00617 }
00618 
00619 
00620 static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
00621 {
00622   bool indirect_index = true;
00623   if(!indices) indirect_index = false;
00624   
00625   FCL_REAL maxD = 0;
00626   for(int i = 0; i < n; ++i)
00627   {
00628     unsigned int index = indirect_index ? indices[i] : i;
00629     const Triangle& t = ts[index];
00630 
00631     for(int j = 0; j < 3; ++j)
00632     {
00633       int point_id = t[j];
00634       const Vec3f& p = ps[point_id];
00635       
00636       FCL_REAL d = (p - query).sqrLength();
00637       if(d > maxD) maxD = d;
00638     }
00639 
00640     if(ps2)
00641     {
00642       for(int j = 0; j < 3; ++j)
00643       {
00644         int point_id = t[j];
00645         const Vec3f& p = ps2[point_id];
00646         
00647         FCL_REAL d = (p - query).sqrLength();
00648         if(d > maxD) maxD = d;
00649       }
00650     }
00651   }
00652 
00653   return std::sqrt(maxD);
00654 }
00655 
00656 static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
00657 {
00658   bool indirect_index = true;
00659   if(!indices) indirect_index = false;
00660 
00661   FCL_REAL maxD = 0;
00662   for(unsigned int i = 0; i < n; ++i)
00663   {
00664     int index = indirect_index ? indices[i] : i;
00665 
00666     const Vec3f& p = ps[index];
00667     FCL_REAL d = (p - query).sqrLength();
00668     if(d > maxD) maxD = d;
00669 
00670     if(ps2)
00671     {
00672       const Vec3f& v = ps2[index];
00673       FCL_REAL d = (v - query).sqrLength();
00674       if(d > maxD) maxD = d;
00675     }
00676   }
00677 
00678   return std::sqrt(maxD);
00679 }
00680 
00681 FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
00682 {
00683   if(ts)
00684     return maximumDistance_mesh(ps, ps2, ts, indices, n, query);
00685   else
00686     return maximumDistance_pointcloud(ps, ps2, indices, n, query);
00687 }
00688 
00689 
00690 }
 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