fcl » include » fcl » BV
kIOS.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_KIOS_H
00038 #define FCL_KIOS_H
00039 
00040 #include "fcl/BV/OBB.h"
00041 
00042 
00043 namespace fcl
00044 {
00045  
00047 class kIOS
00048 {
00050   struct kIOS_Sphere
00051   {
00052     Vec3f o;
00053     FCL_REAL r;
00054   };
00055 
00057   static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
00058   {
00059     Vec3f d = s1.o - s0.o;
00060     FCL_REAL dist2 = d.sqrLength();
00061     FCL_REAL diff_r = s1.r - s0.r;
00062       
00064     if(diff_r * diff_r >= dist2)
00065     {
00066       if(s1.r > s0.r) return s1;
00067       else return s0;
00068     }
00069     else 
00070     {
00071       float dist = std::sqrt(dist2);
00072       kIOS_Sphere s;
00073       s.r = dist + s0.r + s1.r;
00074       if(dist > 0)
00075         s.o = s0.o + d * ((s.r - s0.r) / dist);
00076       else
00077         s.o = s0.o;
00078       return s;
00079     }
00080   }
00081 public:
00082     
00084   kIOS_Sphere spheres[5];
00085 
00087   unsigned int num_spheres;
00088 
00090   OBB obb;
00091 
00093   bool overlap(const kIOS& other) const;
00094 
00098   bool overlap(const kIOS& other, kIOS& overlap_part) const
00099   {
00100     return overlap(other);
00101   }
00102 
00104   inline bool contain(const Vec3f& p) const;
00105 
00107   kIOS& operator += (const Vec3f& p);
00108 
00110   kIOS& operator += (const kIOS& other)
00111   {
00112     *this = *this + other;
00113     return *this;
00114   }
00115 
00117   kIOS operator + (const kIOS& other) const;
00118 
00120   const Vec3f& center() const
00121   {
00122     return spheres[0].o;
00123   }
00124 
00126   FCL_REAL width() const;
00127 
00129   FCL_REAL height() const;
00130 
00132   FCL_REAL depth() const;
00133 
00135   FCL_REAL volume() const;
00136 
00138   FCL_REAL size() const;
00139 
00141   FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
00142 
00143 private:    
00144     
00145 };
00146 
00147 
00149 kIOS translate(const kIOS& bv, const Vec3f& t);
00150 
00153 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
00154 
00157 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
00158 
00159 }
00160 
00161 #endif