kDOP.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/kDOP.h"
00038 #include <limits>
00039 #include <iostream>
00040 
00041 namespace fcl
00042 {
00043 
00045 inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv)
00046 {
00047   if(a > b)
00048   {
00049     minv = b;
00050     maxv = a;
00051   }
00052   else
00053   {
00054     minv = a;
00055     maxv = b;
00056   }
00057 }
00059 inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv)
00060 {
00061   if(p > maxv) maxv = p;
00062   if(p < minv) minv = p;
00063 }
00064 
00065 
00067 template<std::size_t N>
00068 void getDistances(const Vec3f& p, FCL_REAL* d) {}
00069 
00071 template<>
00072 inline void getDistances<5>(const Vec3f& p, FCL_REAL* d)
00073 {
00074   d[0] = p[0] + p[1];
00075   d[1] = p[0] + p[2];
00076   d[2] = p[1] + p[2];
00077   d[3] = p[0] - p[1];
00078   d[4] = p[0] - p[2];
00079 }
00080 
00081 template<>
00082 inline void getDistances<6>(const Vec3f& p, FCL_REAL* d)
00083 {
00084   d[0] = p[0] + p[1];
00085   d[1] = p[0] + p[2];
00086   d[2] = p[1] + p[2];
00087   d[3] = p[0] - p[1];
00088   d[4] = p[0] - p[2];
00089   d[5] = p[1] - p[2];
00090 }
00091 
00092 template<>
00093 inline void getDistances<9>(const Vec3f& p, FCL_REAL* d)
00094 {
00095   d[0] = p[0] + p[1];
00096   d[1] = p[0] + p[2];
00097   d[2] = p[1] + p[2];
00098   d[3] = p[0] - p[1];
00099   d[4] = p[0] - p[2];
00100   d[5] = p[1] - p[2];
00101   d[6] = p[0] + p[1] - p[2];
00102   d[7] = p[0] + p[2] - p[1];
00103   d[8] = p[1] + p[2] - p[0];
00104 }
00105 
00106 
00107 
00108 template<size_t N>
00109 KDOP<N>::KDOP()
00110 {
00111   FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
00112   for(size_t i = 0; i < N / 2; ++i)
00113   {
00114     dist_[i] = real_max;
00115     dist_[i + N / 2] = -real_max;
00116   }
00117 }
00118 
00119 template<size_t N>
00120 KDOP<N>::KDOP(const Vec3f& v)
00121 {
00122   for(size_t i = 0; i < 3; ++i)
00123   {
00124     dist_[i] = dist_[N / 2 + i] = v[i];
00125   }
00126 
00127   FCL_REAL d[(N - 6) / 2];
00128   getDistances<(N - 6) / 2>(v, d);
00129   for(size_t i = 0; i < (N - 6) / 2; ++i)
00130   {
00131     dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
00132   }
00133 }
00134 
00135 template<size_t N>
00136 KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b)
00137 {
00138   for(size_t i = 0; i < 3; ++i)
00139   {
00140     minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
00141   }
00142 
00143   FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
00144   getDistances<(N - 6) / 2>(a, ad);
00145   getDistances<(N - 6) / 2>(b, bd);
00146   for(size_t i = 0; i < (N - 6) / 2; ++i)
00147   {
00148     minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
00149   }
00150 }
00151   
00152 template<size_t N>
00153 bool KDOP<N>::overlap(const KDOP<N>& other) const
00154 {
00155   for(size_t i = 0; i < N / 2; ++i)
00156   {
00157     if(dist_[i] > other.dist_[i + N / 2]) return false;
00158     if(dist_[i + N / 2] < other.dist_[i]) return false;
00159   }
00160 
00161   return true;
00162 }
00163 
00164 template<size_t N>
00165 bool KDOP<N>::inside(const Vec3f& p) const
00166 {
00167   for(size_t i = 0; i < 3; ++i)
00168   {
00169     if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
00170       return false;
00171   }
00172 
00173   FCL_REAL d[(N - 6) / 2];
00174   getDistances<(N - 6) / 2>(p, d);
00175   for(size_t i = 0; i < (N - 6) / 2; ++i)
00176   {
00177     if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
00178       return false;
00179   }
00180 
00181   return true;
00182 }
00183 
00184 template<size_t N>
00185 KDOP<N>& KDOP<N>::operator += (const Vec3f& p)
00186 {
00187   for(size_t i = 0; i < 3; ++i)
00188   {
00189     minmax(p[i], dist_[i], dist_[N / 2 + i]);
00190   }
00191     
00192   FCL_REAL pd[(N - 6) / 2];
00193   getDistances<(N - 6) / 2>(p, pd);
00194   for(size_t i = 0; i < (N - 6) / 2; ++i)
00195   {
00196     minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
00197   }
00198 
00199   return *this;
00200 }
00201 
00202 template<size_t N>
00203 KDOP<N>& KDOP<N>::operator += (const KDOP<N>& other)
00204 {
00205   for(size_t i = 0; i < N / 2; ++i)
00206   {
00207     dist_[i] = std::min(other.dist_[i], dist_[i]);
00208     dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
00209   }
00210   return *this;
00211 }
00212 
00213 template<size_t N>
00214 KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
00215 {
00216   KDOP<N> res(*this);
00217   return res += other;
00218 }
00219 
00220 
00221 template<size_t N>
00222 FCL_REAL KDOP<N>::distance(const KDOP<N>& other, Vec3f* P, Vec3f* Q) const
00223 {
00224   std::cerr << "KDOP distance not implemented!" << std::endl;
00225   return 0.0;
00226 }
00227 
00228 
00229 template<size_t N>
00230 KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t)
00231 {
00232   KDOP<N> res(bv);
00233   for(size_t i = 0; i < 3; ++i)
00234   {
00235     res.dist(i) += t[i];
00236     res.dist(N / 2 + i) += t[i];
00237   }
00238 
00239   FCL_REAL d[(N - 6) / 2];
00240   getDistances<(N - 6) / 2>(t, d);
00241   for(size_t i = 0; i < (N - 6) / 2; ++i)
00242   {
00243     res.dist(3 + i) += d[i];
00244     res.dist(3 + i + N / 2) += d[i];
00245   }
00246 
00247   return res;
00248 }
00249 
00250 
00251 template class KDOP<16>;
00252 template class KDOP<18>;
00253 template class KDOP<24>;
00254 
00255 template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&);
00256 template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&);
00257 template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&);
00258 
00259 }
 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