OBB.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/BV/OBB.h"
00038 #include "fcl/BVH/BVH_utility.h"
00039 #include "fcl/math/transform.h"
00040 
00041 #include <iostream>
00042 #include <limits>
00043 
00044 namespace fcl
00045 {
00046 
00048 inline void computeVertices(const OBB& b, Vec3f vertices[8])
00049 {
00050   const Vec3f* axis = b.axis;
00051   const Vec3f& extent = b.extent;
00052   const Vec3f& To = b.To;
00053 
00054   Vec3f extAxis0 = axis[0] * extent[0];
00055   Vec3f extAxis1 = axis[1] * extent[1];
00056   Vec3f extAxis2 = axis[2] * extent[2];
00057 
00058   vertices[0] = To - extAxis0 - extAxis1 - extAxis2;
00059   vertices[1] = To + extAxis0 - extAxis1 - extAxis2;
00060   vertices[2] = To + extAxis0 + extAxis1 - extAxis2;
00061   vertices[3] = To - extAxis0 + extAxis1 - extAxis2;
00062   vertices[4] = To - extAxis0 - extAxis1 + extAxis2;
00063   vertices[5] = To + extAxis0 - extAxis1 + extAxis2;
00064   vertices[6] = To + extAxis0 + extAxis1 + extAxis2;
00065   vertices[7] = To - extAxis0 + extAxis1 + extAxis2;
00066 }
00067 
00069 inline OBB merge_largedist(const OBB& b1, const OBB& b2)
00070 {
00071   OBB b;
00072   Vec3f vertex[16];
00073   computeVertices(b1, vertex);
00074   computeVertices(b2, vertex + 8);
00075   Matrix3f M;
00076   Vec3f E[3];
00077   Matrix3f::U s[3] = {0, 0, 0};
00078 
00079   // obb axes
00080   Vec3f& R0 = b.axis[0];
00081   Vec3f& R1 = b.axis[1];
00082   Vec3f& R2 = b.axis[2];
00083 
00084   R0 = b1.To - b2.To;
00085   R0.normalize();
00086 
00087   Vec3f vertex_proj[16];
00088   for(int i = 0; i < 16; ++i)
00089     vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
00090 
00091   getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
00092   eigen(M, s, E);
00093 
00094   int min, mid, max;
00095   if (s[0] > s[1]) { max = 0; min = 1; }
00096   else { min = 0; max = 1; }
00097   if (s[2] < s[min]) { mid = min; min = 2; }
00098   else if (s[2] > s[max]) { mid = max; max = 2; }
00099   else { mid = 2; }
00100 
00101 
00102   R1.setValue(E[0][max], E[1][max], E[2][max]);
00103   R2.setValue(E[0][mid], E[1][mid], E[2][mid]);
00104 
00105   // set obb centers and extensions
00106   Vec3f center, extent;
00107   getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent);
00108 
00109   b.To = center;
00110   b.extent = extent;
00111 
00112   return b;
00113 }
00114 
00115 
00117 inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
00118 {
00119   OBB b;
00120   b.To = (b1.To + b2.To) * 0.5;
00121   Quaternion3f q0, q1;
00122   q0.fromAxes(b1.axis);
00123   q1.fromAxes(b2.axis);
00124   if(q0.dot(q1) < 0)
00125     q1 = -q1;
00126 
00127   Quaternion3f q = q0 + q1;
00128   FCL_REAL inv_length = 1.0 / std::sqrt(q.dot(q));
00129   q = q * inv_length;
00130   q.toAxes(b.axis);
00131 
00132 
00133   Vec3f vertex[8], diff;
00134   FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
00135   Vec3f pmin(real_max, real_max, real_max);
00136   Vec3f pmax(-real_max, -real_max, -real_max);
00137 
00138   computeVertices(b1, vertex);
00139   for(int i = 0; i < 8; ++i)
00140   {
00141     diff = vertex[i] - b.To;
00142     for(int j = 0; j < 3; ++j)
00143     {
00144       FCL_REAL dot = diff.dot(b.axis[j]);
00145       if(dot > pmax[j])
00146         pmax[j] = dot;
00147       else if(dot < pmin[j])
00148         pmin[j] = dot;
00149     }
00150   }
00151 
00152   computeVertices(b2, vertex);
00153   for(int i = 0; i < 8; ++i)
00154   {
00155     diff = vertex[i] - b.To;
00156     for(int j = 0; j < 3; ++j)
00157     {
00158       FCL_REAL dot = diff.dot(b.axis[j]);
00159       if(dot > pmax[j])
00160         pmax[j] = dot;
00161       else if(dot < pmin[j])
00162         pmin[j] = dot;
00163     }
00164   }
00165 
00166   for(int j = 0; j < 3; ++j)
00167   {
00168     b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
00169     b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
00170   }
00171 
00172   return b;
00173 }
00174 
00175 bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
00176 {
00177   register FCL_REAL t, s;
00178   const FCL_REAL reps = 1e-6;
00179 
00180   Matrix3f Bf = abs(B);
00181   Bf += reps;
00182 
00183   // if any of these tests are one-sided, then the polyhedra are disjoint
00184 
00185   // A1 x A2 = A0
00186   t = ((T[0] < 0.0) ? -T[0] : T[0]);
00187 
00188   if(t > (a[0] + Bf.dotX(b)))
00189     return true;
00190 
00191   // B1 x B2 = B0
00192   s =  B.transposeDotX(T);
00193   t = ((s < 0.0) ? -s : s);
00194 
00195   if(t > (b[0] + Bf.transposeDotX(a)))
00196     return true;
00197 
00198   // A2 x A0 = A1
00199   t = ((T[1] < 0.0) ? -T[1] : T[1]);
00200 
00201   if(t > (a[1] + Bf.dotY(b)))
00202     return true;
00203 
00204   // A0 x A1 = A2
00205   t =((T[2] < 0.0) ? -T[2] : T[2]);
00206 
00207   if(t > (a[2] + Bf.dotZ(b)))
00208     return true;
00209 
00210   // B2 x B0 = B1
00211   s = B.transposeDotY(T);
00212   t = ((s < 0.0) ? -s : s);
00213 
00214   if(t > (b[1] + Bf.transposeDotY(a)))
00215     return true;
00216 
00217   // B0 x B1 = B2
00218   s = B.transposeDotZ(T);
00219   t = ((s < 0.0) ? -s : s);
00220 
00221   if(t > (b[2] + Bf.transposeDotZ(a)))
00222     return true;
00223 
00224   // A0 x B0
00225   s = T[2] * B(1, 0) - T[1] * B(2, 0);
00226   t = ((s < 0.0) ? -s : s);
00227 
00228   if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
00229           b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
00230     return true;
00231 
00232   // A0 x B1
00233   s = T[2] * B(1, 1) - T[1] * B(2, 1);
00234   t = ((s < 0.0) ? -s : s);
00235 
00236   if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
00237           b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
00238     return true;
00239 
00240   // A0 x B2
00241   s = T[2] * B(1, 2) - T[1] * B(2, 2);
00242   t = ((s < 0.0) ? -s : s);
00243 
00244   if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
00245           b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
00246     return true;
00247 
00248   // A1 x B0
00249   s = T[0] * B(2, 0) - T[2] * B(0, 0);
00250   t = ((s < 0.0) ? -s : s);
00251 
00252   if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
00253           b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
00254     return true;
00255 
00256   // A1 x B1
00257   s = T[0] * B(2, 1) - T[2] * B(0, 1);
00258   t = ((s < 0.0) ? -s : s);
00259 
00260   if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
00261           b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
00262     return true;
00263 
00264   // A1 x B2
00265   s = T[0] * B(2, 2) - T[2] * B(0, 2);
00266   t = ((s < 0.0) ? -s : s);
00267 
00268   if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
00269           b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
00270     return true;
00271 
00272   // A2 x B0
00273   s = T[1] * B(0, 0) - T[0] * B(1, 0);
00274   t = ((s < 0.0) ? -s : s);
00275 
00276   if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
00277           b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
00278     return true;
00279 
00280   // A2 x B1
00281   s = T[1] * B(0, 1) - T[0] * B(1, 1);
00282   t = ((s < 0.0) ? -s : s);
00283 
00284   if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
00285           b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
00286     return true;
00287 
00288   // A2 x B2
00289   s = T[1] * B(0, 2) - T[0] * B(1, 2);
00290   t = ((s < 0.0) ? -s : s);
00291 
00292   if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
00293           b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
00294     return true;
00295 
00296   return false;
00297 
00298 }
00299 
00300 
00301 
00302 bool OBB::overlap(const OBB& other) const
00303 {
00307   Vec3f t = other.To - To; // T2 - T1
00308   Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
00309   Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
00310              axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
00311              axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
00312 
00313   return !obbDisjoint(R, T, extent, other.extent);
00314 }
00315 
00316 
00317 bool OBB::contain(const Vec3f& p) const
00318 {
00319   Vec3f local_p = p - To;
00320   FCL_REAL proj = local_p.dot(axis[0]);
00321   if((proj > extent[0]) || (proj < -extent[0]))
00322     return false;
00323 
00324   proj = local_p.dot(axis[1]);
00325   if((proj > extent[1]) || (proj < -extent[1]))
00326     return false;
00327 
00328   proj = local_p.dot(axis[2]);
00329   if((proj > extent[2]) || (proj < -extent[2]))
00330     return false;
00331 
00332   return true;
00333 }
00334 
00335 OBB& OBB::operator += (const Vec3f& p)
00336 {
00337   OBB bvp;
00338   bvp.To = p;
00339   bvp.axis[0] = axis[0];
00340   bvp.axis[1] = axis[1];
00341   bvp.axis[2] = axis[2];
00342   bvp.extent.setValue(0);
00343 
00344   *this += bvp;
00345   return *this;
00346 }
00347 
00348 OBB OBB::operator + (const OBB& other) const
00349 {
00350   Vec3f center_diff = To - other.To;
00351   FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
00352   FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
00353   if(center_diff.length() > 2 * (max_extent + max_extent2))
00354   {
00355     return merge_largedist(*this, other);
00356   }
00357   else
00358   {
00359     return merge_smalldist(*this, other);
00360   }
00361 }
00362 
00363 
00364 FCL_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
00365 {
00366   std::cerr << "OBB distance not implemented!" << std::endl;
00367   return 0.0;
00368 }
00369 
00370 
00371 bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
00372 {
00373   Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
00374                 R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
00375                 R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
00376 
00377   Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
00378              R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
00379              R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
00380 
00381   Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
00382   Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
00383 
00384   return !obbDisjoint(R, T, b1.extent, b2.extent);
00385 }
00386 
00387 OBB translate(const OBB& bv, const Vec3f& t)
00388 {
00389   OBB res(bv);
00390   res.To += t;
00391   return res;
00392 }
00393 
00394 }
 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