geometric_shapes_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/shape/geometric_shapes_utility.h"
00039 #include "fcl/BVH/BV_fitter.h"
00040 
00041 namespace fcl
00042 {
00043 
00044 namespace details
00045 {
00046 
00047 std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf)
00048 {
00049   std::vector<Vec3f> result(8);
00050   FCL_REAL a = box.side[0] / 2;
00051   FCL_REAL b = box.side[1] / 2;
00052   FCL_REAL c = box.side[2] / 2;
00053   result[0] = tf.transform(Vec3f(a, b, c));
00054   result[1] = tf.transform(Vec3f(a, b, -c));
00055   result[2] = tf.transform(Vec3f(a, -b, c));
00056   result[3] = tf.transform(Vec3f(a, -b, -c));
00057   result[4] = tf.transform(Vec3f(-a, b, c));
00058   result[5] = tf.transform(Vec3f(-a, b, -c));
00059   result[6] = tf.transform(Vec3f(-a, -b, c));
00060   result[7] = tf.transform(Vec3f(-a, -b, -c));
00061   
00062   return result;
00063 }
00064 
00065 // we use icosahedron to bound the sphere
00066 std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
00067 {
00068   std::vector<Vec3f> result(12);
00069   const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
00070   FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
00071   
00072   FCL_REAL a = edge_size;
00073   FCL_REAL b = m * edge_size;
00074   result[0] = tf.transform(Vec3f(0, a, b));
00075   result[1] = tf.transform(Vec3f(0, -a, b));
00076   result[2] = tf.transform(Vec3f(0, a, -b));
00077   result[3] = tf.transform(Vec3f(0, -a, -b));
00078   result[4] = tf.transform(Vec3f(a, b, 0));
00079   result[5] = tf.transform(Vec3f(-a, b, 0));
00080   result[6] = tf.transform(Vec3f(a, -b, 0));
00081   result[7] = tf.transform(Vec3f(-a, -b, 0));
00082   result[8] = tf.transform(Vec3f(b, 0, a));
00083   result[9] = tf.transform(Vec3f(b, 0, -a));
00084   result[10] = tf.transform(Vec3f(-b, 0, a));
00085   result[11] = tf.transform(Vec3f(-b, 0, -a));
00086 
00087   return result;
00088 }
00089 
00090 std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf)
00091 {
00092   std::vector<Vec3f> result(36);
00093   const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
00094 
00095   FCL_REAL hl = capsule.lz * 0.5;
00096   FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
00097   FCL_REAL a = edge_size;
00098   FCL_REAL b = m * edge_size;
00099   FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
00100 
00101 
00102   result[0] = tf.transform(Vec3f(0, a, b + hl));
00103   result[1] = tf.transform(Vec3f(0, -a, b + hl));
00104   result[2] = tf.transform(Vec3f(0, a, -b + hl));
00105   result[3] = tf.transform(Vec3f(0, -a, -b + hl));
00106   result[4] = tf.transform(Vec3f(a, b, hl));
00107   result[5] = tf.transform(Vec3f(-a, b, hl));
00108   result[6] = tf.transform(Vec3f(a, -b, hl));
00109   result[7] = tf.transform(Vec3f(-a, -b, hl));
00110   result[8] = tf.transform(Vec3f(b, 0, a + hl));
00111   result[9] = tf.transform(Vec3f(b, 0, -a + hl));
00112   result[10] = tf.transform(Vec3f(-b, 0, a + hl));
00113   result[11] = tf.transform(Vec3f(-b, 0, -a + hl));
00114 
00115   result[12] = tf.transform(Vec3f(0, a, b - hl));
00116   result[13] = tf.transform(Vec3f(0, -a, b - hl));
00117   result[14] = tf.transform(Vec3f(0, a, -b - hl));
00118   result[15] = tf.transform(Vec3f(0, -a, -b - hl));
00119   result[16] = tf.transform(Vec3f(a, b, -hl));
00120   result[17] = tf.transform(Vec3f(-a, b, -hl));
00121   result[18] = tf.transform(Vec3f(a, -b, -hl));
00122   result[19] = tf.transform(Vec3f(-a, -b, -hl));
00123   result[20] = tf.transform(Vec3f(b, 0, a - hl));
00124   result[21] = tf.transform(Vec3f(b, 0, -a - hl));
00125   result[22] = tf.transform(Vec3f(-b, 0, a - hl));
00126   result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
00127 
00128   FCL_REAL c = 0.5 * r2;
00129   FCL_REAL d = capsule.radius;
00130   result[24] = tf.transform(Vec3f(r2, 0, hl));
00131   result[25] = tf.transform(Vec3f(c, d, hl));
00132   result[26] = tf.transform(Vec3f(-c, d, hl));
00133   result[27] = tf.transform(Vec3f(-r2, 0, hl));
00134   result[28] = tf.transform(Vec3f(-c, -d, hl));
00135   result[29] = tf.transform(Vec3f(c, -d, hl));
00136 
00137   result[30] = tf.transform(Vec3f(r2, 0, -hl));
00138   result[31] = tf.transform(Vec3f(c, d, -hl));
00139   result[32] = tf.transform(Vec3f(-c, d, -hl));
00140   result[33] = tf.transform(Vec3f(-r2, 0, -hl));
00141   result[34] = tf.transform(Vec3f(-c, -d, -hl));
00142   result[35] = tf.transform(Vec3f(c, -d, -hl));
00143 
00144   return result;
00145 }
00146 
00147 
00148 std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
00149 {
00150   std::vector<Vec3f> result(7);
00151   
00152   FCL_REAL hl = cone.lz * 0.5;
00153   FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
00154   FCL_REAL a = 0.5 * r2;
00155   FCL_REAL b = cone.radius;
00156 
00157   result[0] = tf.transform(Vec3f(r2, 0, -hl));
00158   result[1] = tf.transform(Vec3f(a, b, -hl));
00159   result[2] = tf.transform(Vec3f(-a, b, -hl));
00160   result[3] = tf.transform(Vec3f(-r2, 0, -hl));
00161   result[4] = tf.transform(Vec3f(-a, -b, -hl));
00162   result[5] = tf.transform(Vec3f(a, -b, -hl));
00163 
00164   result[6] = tf.transform(Vec3f(0, 0, hl));
00165                           
00166   return result;
00167 }
00168 
00169 std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf)
00170 {
00171   std::vector<Vec3f> result(12);
00172 
00173   FCL_REAL hl = cylinder.lz * 0.5;
00174   FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
00175   FCL_REAL a = 0.5 * r2;
00176   FCL_REAL b = cylinder.radius;
00177 
00178   result[0] = tf.transform(Vec3f(r2, 0, -hl));
00179   result[1] = tf.transform(Vec3f(a, b, -hl));
00180   result[2] = tf.transform(Vec3f(-a, b, -hl));
00181   result[3] = tf.transform(Vec3f(-r2, 0, -hl));
00182   result[4] = tf.transform(Vec3f(-a, -b, -hl));
00183   result[5] = tf.transform(Vec3f(a, -b, -hl));
00184 
00185   result[6] = tf.transform(Vec3f(r2, 0, hl));
00186   result[7] = tf.transform(Vec3f(a, b, hl));
00187   result[8] = tf.transform(Vec3f(-a, b, hl));
00188   result[9] = tf.transform(Vec3f(-r2, 0, hl));
00189   result[10] = tf.transform(Vec3f(-a, -b, hl));
00190   result[11] = tf.transform(Vec3f(a, -b, hl));
00191 
00192   return result;
00193 }
00194 
00195 std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf)
00196 {
00197   std::vector<Vec3f> result(convex.num_points);
00198   for(int i = 0; i < convex.num_points; ++i)
00199   {
00200     result[i] = tf.transform(convex.points[i]);
00201   }
00202 
00203   return result;
00204 }
00205 
00206 std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf)
00207 {
00208   std::vector<Vec3f> result(3);
00209   result[0] = tf.transform(triangle.a);
00210   result[1] = tf.transform(triangle.b);
00211   result[2] = tf.transform(triangle.c);
00212 
00213   return result;
00214 }
00215 
00216 } // end detail
00217 
00218 Halfspace transform(const Halfspace& a, const Transform3f& tf)
00219 {
00225 
00226   Vec3f n = tf.getQuatRotation().transform(a.n);
00227   FCL_REAL d = a.d + n.dot(tf.getTranslation());
00228 
00229   return Halfspace(n, d);
00230 }
00231 
00232 
00233 Plane transform(const Plane& a, const Transform3f& tf)
00234 {
00240 
00241   Vec3f n = tf.getQuatRotation().transform(a.n);
00242   FCL_REAL d = a.d + n.dot(tf.getTranslation());
00243 
00244   return Plane(n, d);
00245 }
00246 
00247 
00248 
00249 template<>
00250 void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
00251 {
00252   const Matrix3f& R = tf.getRotation();
00253   const Vec3f& T = tf.getTranslation();
00254 
00255   FCL_REAL x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2]));
00256   FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2]));
00257   FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2]));
00258 
00259   Vec3f v_delta(x_range, y_range, z_range);
00260   bv.max_ = T + v_delta;
00261   bv.min_ = T - v_delta;
00262 }
00263 
00264 template<>
00265 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
00266 {
00267   const Vec3f& T = tf.getTranslation();
00268 
00269   Vec3f v_delta(s.radius);
00270   bv.max_ = T + v_delta;
00271   bv.min_ = T - v_delta;
00272 }
00273 
00274 template<>
00275 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
00276 {
00277   const Matrix3f& R = tf.getRotation();
00278   const Vec3f& T = tf.getTranslation();
00279 
00280   FCL_REAL x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius;
00281   FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
00282   FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
00283 
00284   Vec3f v_delta(x_range, y_range, z_range);
00285   bv.max_ = T + v_delta;
00286   bv.min_ = T - v_delta;
00287 }
00288 
00289 template<>
00290 void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
00291 {
00292   const Matrix3f& R = tf.getRotation();
00293   const Vec3f& T = tf.getTranslation();
00294 
00295   FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
00296   FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
00297   FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
00298 
00299   Vec3f v_delta(x_range, y_range, z_range);
00300   bv.max_ = T + v_delta;
00301   bv.min_ = T - v_delta;
00302 }
00303 
00304 template<>
00305 void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv)
00306 {
00307   const Matrix3f& R = tf.getRotation();
00308   const Vec3f& T = tf.getTranslation();
00309 
00310   FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
00311   FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
00312   FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
00313 
00314   Vec3f v_delta(x_range, y_range, z_range);
00315   bv.max_ = T + v_delta;
00316   bv.min_ = T - v_delta;
00317 }
00318 
00319 template<>
00320 void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv)
00321 {
00322   const Matrix3f& R = tf.getRotation();
00323   const Vec3f& T = tf.getTranslation();
00324 
00325   AABB bv_;
00326   for(int i = 0; i < s.num_points; ++i)
00327   {
00328     Vec3f new_p = R * s.points[i] + T;
00329     bv_ += new_p;
00330   }
00331 
00332   bv = bv_;
00333 }
00334 
00335 template<>
00336 void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv)
00337 {
00338   bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
00339 }
00340 
00341 
00342 template<>
00343 void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv)
00344 {
00345   Halfspace new_s = transform(s, tf);
00346   const Vec3f& n = new_s.n;
00347   const FCL_REAL& d = new_s.d;
00348 
00349   AABB bv_;
00350   bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
00351   bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
00352   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00353   {
00354     // normal aligned with x axis
00355     if(n[0] < 0) bv_.min_[0] = -d;
00356     else if(n[0] > 0) bv_.max_[0] = d;
00357   }
00358   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00359   {
00360     // normal aligned with y axis
00361     if(n[1] < 0) bv_.min_[1] = -d;
00362     else if(n[1] > 0) bv_.max_[1] = d;
00363   }
00364   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00365   {
00366     // normal aligned with z axis
00367     if(n[2] < 0) bv_.min_[2] = -d;
00368     else if(n[2] > 0) bv_.max_[2] = d;
00369   }
00370 
00371   bv = bv_;  
00372 }
00373 
00374 template<>
00375 void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
00376 {
00377   Plane new_s = transform(s, tf);
00378   const Vec3f& n = new_s.n;
00379   const FCL_REAL& d = new_s.d;  
00380 
00381   AABB bv_;
00382   bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
00383   bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
00384   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00385   {
00386     // normal aligned with x axis
00387     if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; }
00388     else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; }
00389   }
00390   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00391   {
00392     // normal aligned with y axis
00393     if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; }
00394     else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; }
00395   }
00396   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00397   {
00398     // normal aligned with z axis
00399     if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; }
00400     else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; }
00401   }
00402 
00403   bv = bv_;
00404 }
00405 
00406 
00407 template<>
00408 void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
00409 {
00410   const Matrix3f& R = tf.getRotation();
00411   const Vec3f& T = tf.getTranslation();
00412 
00413   bv.To = T;
00414   bv.axis[0] = R.getColumn(0);
00415   bv.axis[1] = R.getColumn(1);
00416   bv.axis[2] = R.getColumn(2);
00417   bv.extent = s.side * (FCL_REAL)0.5;
00418 }
00419 
00420 template<>
00421 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
00422 {
00423   const Vec3f& T = tf.getTranslation();
00424 
00425   bv.To = T;
00426   bv.axis[0].setValue(1, 0, 0);
00427   bv.axis[1].setValue(0, 1, 0);
00428   bv.axis[2].setValue(0, 0, 1);
00429   bv.extent.setValue(s.radius);
00430 }
00431 
00432 template<>
00433 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
00434 {
00435   const Matrix3f& R = tf.getRotation();
00436   const Vec3f& T = tf.getTranslation();
00437 
00438   bv.To = T;
00439   bv.axis[0] = R.getColumn(0);
00440   bv.axis[1] = R.getColumn(1);
00441   bv.axis[2] = R.getColumn(2);
00442   bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius);
00443 }
00444 
00445 template<>
00446 void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
00447 {
00448   const Matrix3f& R = tf.getRotation();
00449   const Vec3f& T = tf.getTranslation();
00450 
00451   bv.To = T;
00452   bv.axis[0] = R.getColumn(0);
00453   bv.axis[1] = R.getColumn(1);
00454   bv.axis[2] = R.getColumn(2);
00455   bv.extent.setValue(s.radius, s.radius, s.lz / 2);
00456 }
00457 
00458 template<>
00459 void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
00460 {
00461   const Matrix3f& R = tf.getRotation();
00462   const Vec3f& T = tf.getTranslation();
00463 
00464   bv.To = T;
00465   bv.axis[0] = R.getColumn(0);
00466   bv.axis[1] = R.getColumn(1);
00467   bv.axis[2] = R.getColumn(2);
00468   bv.extent.setValue(s.radius, s.radius, s.lz / 2);
00469 }
00470 
00471 template<>
00472 void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
00473 {
00474   const Matrix3f& R = tf.getRotation();
00475   const Vec3f& T = tf.getTranslation();
00476 
00477   fit(s.points, s.num_points, bv);
00478 
00479   bv.axis[0] = R * bv.axis[0];
00480   bv.axis[1] = R * bv.axis[1];
00481   bv.axis[2] = R * bv.axis[2];
00482 
00483   bv.To = R * bv.To + T;
00484 }
00485 
00486 template<>
00487 void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv)
00488 {
00490   bv.axis[0] = Vec3f(1, 0, 0);
00491   bv.axis[1] = Vec3f(0, 1, 0);
00492   bv.axis[2] = Vec3f(0, 0, 1);
00493   bv.To = Vec3f(0, 0, 0);
00494   bv.extent.setValue(std::numeric_limits<FCL_REAL>::max());
00495 }
00496 
00497 template<>
00498 void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv)
00499 {
00501   bv.axis[0] = Vec3f(1, 0, 0);
00502   bv.axis[1] = Vec3f(0, 1, 0);
00503   bv.axis[2] = Vec3f(0, 0, 1);
00504   bv.Tr = Vec3f(0, 0, 0);
00505   bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
00506 }
00507 
00508 template<>
00509 void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv)
00510 {
00511   computeBV<OBB, Halfspace>(s, tf, bv.obb);
00512   computeBV<RSS, Halfspace>(s, tf, bv.rss);
00513 }
00514 
00515 template<>
00516 void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv)
00517 {
00518   bv.num_spheres = 1;
00519   computeBV<OBB, Halfspace>(s, tf, bv.obb);
00520   bv.spheres[0].o = Vec3f();
00521   bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
00522 }
00523 
00524 template<>
00525 void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv)
00526 {
00527   Halfspace new_s = transform(s, tf);
00528   const Vec3f& n = new_s.n;
00529   const FCL_REAL& d = new_s.d;
00530 
00531   const std::size_t D = 8;
00532   for(std::size_t i = 0; i < D; ++i)
00533     bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00534   for(std::size_t i = D; i < 2 * D; ++i)
00535     bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00536   
00537   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00538   {
00539     if(n[0] > 0) bv.dist(D) = d;
00540     else bv.dist(0) = -d;
00541   }
00542   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00543   {
00544     if(n[1] > 0) bv.dist(D + 1) = d;
00545     else bv.dist(1) = -d;
00546   }
00547   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00548   {
00549     if(n[2] > 0) bv.dist(D + 2) = d;
00550     else bv.dist(2) = -d;
00551   }
00552   else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00553   {
00554     if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
00555     else bv.dist(3) = n[0] * d * 2;
00556   }
00557   else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00558   {
00559     if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
00560     else bv.dist(4) = n[0] * d * 2;
00561   }
00562   else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00563   {
00564     if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
00565     else bv.dist(5) = n[1] * d * 2;
00566   }
00567   else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00568   {
00569     if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
00570     else bv.dist(6) = n[0] * d * 2;
00571   }
00572   else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00573   {
00574     if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
00575     else bv.dist(7) = n[0] * d * 2;
00576   }
00577 }
00578 
00579 template<>
00580 void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv)
00581 {
00582   Halfspace new_s = transform(s, tf);
00583   const Vec3f& n = new_s.n;
00584   const FCL_REAL& d = new_s.d;
00585 
00586   const std::size_t D = 9;
00587 
00588   for(std::size_t i = 0; i < D; ++i)
00589     bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00590   for(std::size_t i = D; i < 2 * D; ++i)
00591     bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00592   
00593   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00594   {
00595     if(n[0] > 0) bv.dist(D) = d;
00596     else bv.dist(0) = -d;
00597   }
00598   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00599   {
00600     if(n[1] > 0) bv.dist(D + 1) = d;
00601     else bv.dist(1) = -d;
00602   }
00603   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00604   {
00605     if(n[2] > 0) bv.dist(D + 2) = d;
00606     else bv.dist(2) = -d;
00607   }
00608   else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00609   {
00610     if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
00611     else bv.dist(3) = n[0] * d * 2;
00612   }
00613   else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00614   {
00615     if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
00616     else bv.dist(4) = n[0] * d * 2;
00617   }
00618   else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00619   {
00620     if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
00621     else bv.dist(5) = n[1] * d * 2;
00622   }
00623   else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00624   {
00625     if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
00626     else bv.dist(6) = n[0] * d * 2;
00627   }
00628   else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00629   {
00630     if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
00631     else bv.dist(7) = n[0] * d * 2;
00632   }
00633   else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00634   {
00635     if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
00636     else bv.dist(8) = n[1] * d * 2;
00637   }
00638 }
00639 
00640 template<>
00641 void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv)
00642 {
00643   Halfspace new_s = transform(s, tf);
00644   const Vec3f& n = new_s.n;
00645   const FCL_REAL& d = new_s.d;
00646 
00647   const std::size_t D = 12;
00648 
00649   for(std::size_t i = 0; i < D; ++i)
00650     bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00651   for(std::size_t i = D; i < 2 * D; ++i)
00652     bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00653   
00654   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00655   {
00656     if(n[0] > 0) bv.dist(D) = d;
00657     else bv.dist(0) = -d;
00658   }
00659   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00660   {
00661     if(n[1] > 0) bv.dist(D + 1) = d;
00662     else bv.dist(1) = -d;
00663   }
00664   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00665   {
00666     if(n[2] > 0) bv.dist(D + 2) = d;
00667     else bv.dist(2) = -d;
00668   }
00669   else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00670   {
00671     if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
00672     else bv.dist(3) = n[0] * d * 2;
00673   }
00674   else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00675   {
00676     if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
00677     else bv.dist(4) = n[0] * d * 2;
00678   }
00679   else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00680   {
00681     if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
00682     else bv.dist(5) = n[1] * d * 2;
00683   }
00684   else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00685   {
00686     if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
00687     else bv.dist(6) = n[0] * d * 2;
00688   }
00689   else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00690   {
00691     if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
00692     else bv.dist(7) = n[0] * d * 2;
00693   }
00694   else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00695   {
00696     if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
00697     else bv.dist(8) = n[1] * d * 2;
00698   }
00699   else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00700   {
00701     if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3;
00702     else bv.dist(9) = n[0] * d * 3;
00703   }
00704   else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00705   {
00706     if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3;
00707     else bv.dist(10) = n[0] * d * 3;
00708   }
00709   else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00710   {
00711     if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3;
00712     else bv.dist(11) = n[1] * d * 3;
00713   }
00714 }
00715 
00716 
00717 
00718 template<>
00719 void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
00720 {
00721   Vec3f n = tf.getQuatRotation().transform(s.n);
00722   generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
00723   bv.axis[0] = n;
00724 
00725   bv.extent.setValue(0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max());
00726 
00727   Vec3f p = s.n * s.d; 
00728   bv.To = tf.transform(p); 
00729 }
00730 
00731 template<>
00732 void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
00733 {
00734   Vec3f n = tf.getQuatRotation().transform(s.n);
00735 
00736   generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
00737   bv.axis[0] = n;
00738 
00739   bv.l[0] = std::numeric_limits<FCL_REAL>::max();
00740   bv.l[1] = std::numeric_limits<FCL_REAL>::max();
00741 
00742   bv.r = 0;
00743   
00744   Vec3f p = s.n * s.d;
00745   bv.Tr = tf.transform(p);
00746 }
00747 
00748 template<>
00749 void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv)
00750 {
00751   computeBV<OBB, Plane>(s, tf, bv.obb);
00752   computeBV<RSS, Plane>(s, tf, bv.rss);
00753 }
00754 
00755 template<>
00756 void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv)
00757 {
00758   bv.num_spheres = 1;
00759   computeBV<OBB, Plane>(s, tf, bv.obb);
00760   bv.spheres[0].o = Vec3f();
00761   bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
00762 }
00763 
00764 template<>
00765 void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv)
00766 {
00767   Plane new_s = transform(s, tf);
00768   const Vec3f& n = new_s.n;
00769   const FCL_REAL& d = new_s.d;
00770 
00771   const std::size_t D = 8;
00772 
00773   for(std::size_t i = 0; i < D; ++i)
00774     bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00775   for(std::size_t i = D; i < 2 * D; ++i)
00776     bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00777   
00778   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00779   {
00780     if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
00781     else bv.dist(0) = bv.dist(D) = -d;
00782   }
00783   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00784   {
00785     if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
00786     else bv.dist(1) = bv.dist(D + 1) = -d;
00787   }
00788   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00789   {
00790     if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
00791     else bv.dist(2) = bv.dist(D + 2) = -d;
00792   }
00793   else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00794   {
00795     bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
00796   }
00797   else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00798   {
00799     bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
00800   }
00801   else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00802   {
00803     bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
00804   }
00805   else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00806   {
00807     bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
00808   }
00809   else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00810   {
00811     bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
00812   }
00813 }
00814 
00815 template<>
00816 void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv)
00817 {
00818   Plane new_s = transform(s, tf);
00819   const Vec3f& n = new_s.n;
00820   const FCL_REAL& d = new_s.d;
00821 
00822   const std::size_t D = 9;
00823 
00824   for(std::size_t i = 0; i < D; ++i)
00825     bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00826   for(std::size_t i = D; i < 2 * D; ++i)
00827     bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00828   
00829   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00830   {
00831     if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
00832     else bv.dist(0) = bv.dist(D) = -d;
00833   }
00834   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00835   {
00836     if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
00837     else bv.dist(1) = bv.dist(D + 1) = -d;
00838   }
00839   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00840   {
00841     if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
00842     else bv.dist(2) = bv.dist(D + 2) = -d;
00843   }
00844   else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00845   {
00846     bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
00847   }
00848   else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00849   {
00850     bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
00851   }
00852   else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00853   {
00854     bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
00855   }
00856   else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00857   {
00858     bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
00859   }
00860   else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00861   {
00862     bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
00863   }
00864   else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00865   {
00866     bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
00867   }
00868 }
00869 
00870 template<>
00871 void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv)
00872 {
00873   Plane new_s = transform(s, tf);
00874   const Vec3f& n = new_s.n;
00875   const FCL_REAL& d = new_s.d;
00876 
00877   const std::size_t D = 12;
00878 
00879   for(std::size_t i = 0; i < D; ++i)
00880     bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
00881   for(std::size_t i = D; i < 2 * D; ++i)
00882     bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
00883   
00884   if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00885   {
00886     if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
00887     else bv.dist(0) = bv.dist(D) = -d;
00888   }
00889   else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
00890   {
00891     if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
00892     else bv.dist(1) = bv.dist(D + 1) = -d;
00893   }
00894   else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
00895   {
00896     if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
00897     else bv.dist(2) = bv.dist(D + 2) = -d;
00898   }
00899   else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
00900   {
00901     bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
00902   }
00903   else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
00904   {
00905     bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
00906   }
00907   else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
00908   {
00909     bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
00910   }
00911   else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00912   {
00913     bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
00914   }
00915   else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00916   {
00917     bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
00918   }
00919   else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00920   {
00921     bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
00922   }
00923   else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
00924   {
00925     bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
00926   }
00927   else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
00928   {
00929     bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
00930   }
00931   else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
00932   {
00933     bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
00934   }
00935 }
00936 
00937 
00938 void constructBox(const AABB& bv, Box& box, Transform3f& tf)
00939 {
00940   box = Box(bv.max_ - bv.min_);
00941   tf = Transform3f(bv.center());
00942 }
00943 
00944 void constructBox(const OBB& bv, Box& box, Transform3f& tf)
00945 {
00946   box = Box(bv.extent * 2);
00947   tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00948                             bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00949                             bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
00950 }
00951 
00952 void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
00953 {
00954   box = Box(bv.obb.extent * 2);
00955   tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00956                             bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00957                             bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00958 }
00959 
00960 void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
00961 {
00962   box = Box(bv.obb.extent * 2);
00963   tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00964                             bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00965                             bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
00966 }
00967 
00968 void constructBox(const RSS& bv, Box& box, Transform3f& tf)
00969 {
00970   box = Box(bv.width(), bv.height(), bv.depth());
00971   tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00972                             bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00973                             bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
00974 }
00975 
00976 void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
00977 {
00978   box = Box(bv.width(), bv.height(), bv.depth());
00979   tf = Transform3f(bv.center());
00980 }
00981 
00982 void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf)
00983 {
00984   box = Box(bv.width(), bv.height(), bv.depth());
00985   tf = Transform3f(bv.center());
00986 }
00987 
00988 void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf)
00989 {
00990   box = Box(bv.width(), bv.height(), bv.depth());
00991   tf = Transform3f(bv.center());
00992 }
00993 
00994 
00995 
00996 void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
00997 {
00998   box = Box(bv.max_ - bv.min_);
00999   tf = tf_bv * Transform3f(bv.center());
01000 }
01001 
01002 void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01003 {
01004   box = Box(bv.extent * 2);
01005   tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
01006                                    bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
01007                                    bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
01008 }
01009 
01010 void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01011 {
01012   box = Box(bv.obb.extent * 2);
01013   tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
01014                                     bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
01015                                     bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
01016 }
01017 
01018 void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01019 {
01020   box = Box(bv.obb.extent * 2);
01021   tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
01022                                     bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
01023                                     bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
01024 }
01025 
01026 void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01027 {
01028   box = Box(bv.width(), bv.height(), bv.depth());
01029   tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
01030                                     bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
01031                                     bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
01032 }
01033 
01034 void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01035 {
01036   box = Box(bv.width(), bv.height(), bv.depth());
01037   tf = tf_bv * Transform3f(bv.center());
01038 }
01039 
01040 void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01041 {
01042   box = Box(bv.width(), bv.height(), bv.depth());
01043   tf = tf_bv * Transform3f(bv.center());
01044 }
01045 
01046 void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
01047 {
01048   box = Box(bv.width(), bv.height(), bv.depth());
01049   tf = tf_bv * Transform3f(bv.center());
01050 }
01051 
01052 
01053 
01054 }
 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