fcl » include » fcl » BV
kDOP.h

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 #ifndef FCL_KDOP_H
00038 #define FCL_KDOP_H
00039 
00040 
00041 #include "fcl/math/vec_3f.h"
00042 
00043 namespace fcl
00044 {
00045 
00046 
00059 template<size_t N>
00060 class KDOP
00061 {
00062 public:
00063 
00065   KDOP();
00066 
00068   KDOP(const Vec3f& v);
00069 
00071   KDOP(const Vec3f& a, const Vec3f& b);
00072   
00074   bool overlap(const KDOP<N>& other) const;
00075 
00077   bool inside(const Vec3f& p) const;
00078 
00080   KDOP<N>& operator += (const Vec3f& p);
00081 
00083   KDOP<N>& operator += (const KDOP<N>& other);
00084 
00086   KDOP<N> operator + (const KDOP<N>& other) const;
00087 
00089   inline FCL_REAL width() const
00090   {
00091     return dist_[N / 2] - dist_[0];
00092   }
00093 
00095   inline FCL_REAL height() const
00096   {
00097     return dist_[N / 2 + 1] - dist_[1];
00098   }
00099 
00101   inline FCL_REAL depth() const
00102   {
00103     return dist_[N / 2 + 2] - dist_[2];
00104   }
00105 
00107   inline FCL_REAL volume() const
00108   {
00109     return width() * height() * depth();
00110   }
00111 
00113   inline FCL_REAL size() const
00114   {
00115     return width() * width() + height() * height() + depth() * depth();
00116   }
00117 
00119   inline Vec3f center() const
00120   {
00121     return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
00122   }
00123 
00125   FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
00126 
00127 private:
00129   FCL_REAL dist_[N];
00130 
00131 public:
00132   inline FCL_REAL dist(std::size_t i) const
00133   {
00134     return dist_[i];
00135   }
00136 
00137   inline FCL_REAL& dist(std::size_t i)
00138   {
00139     return dist_[i];
00140   }
00141 
00142 
00143 };
00144 
00145 
00147 template<size_t N>
00148 KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
00149 
00150 }
00151 
00152 #endif