kIOS.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/kIOS.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 
00047 bool kIOS::overlap(const kIOS& other) const
00048 {
00049   for(unsigned int i = 0; i < num_spheres; ++i)
00050   {
00051     for(unsigned int j = 0; j < other.num_spheres; ++j)
00052     {
00053       FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).sqrLength();
00054       FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
00055       if(o_dist > sum_r * sum_r)
00056         return false;
00057     }
00058   }
00059 
00060   return obb.overlap(other.obb);
00061 
00062   return true;
00063 }
00064 
00065 
00066 bool kIOS::contain(const Vec3f& p) const
00067 {
00068   for(unsigned int i = 0; i < num_spheres; ++i)
00069   {
00070     FCL_REAL r = spheres[i].r;
00071     if((spheres[i].o - p).sqrLength() > r * r)
00072       return false;
00073   }
00074 
00075   return true;
00076 }
00077 
00078 kIOS& kIOS::operator += (const Vec3f& p)
00079 {
00080   for(unsigned int i = 0; i < num_spheres; ++i)
00081   {
00082     FCL_REAL r = spheres[i].r;
00083     FCL_REAL new_r_sqr = (p - spheres[i].o).sqrLength();
00084     if(new_r_sqr > r * r)
00085     {
00086       spheres[i].r = sqrt(new_r_sqr);
00087     }
00088   }
00089 
00090   obb += p;
00091   return *this;
00092 }
00093 
00094 kIOS kIOS::operator + (const kIOS& other) const
00095 {
00096   kIOS result;
00097   unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres);
00098   for(unsigned int i = 0; i < new_num_spheres; ++i)
00099   {
00100     result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]);
00101   }
00102     
00103   result.num_spheres = new_num_spheres;
00104 
00105   result.obb = obb + other.obb;
00106   
00107   return result;
00108 }
00109 
00110 FCL_REAL kIOS::width() const
00111 {
00112   return obb.width();
00113 }
00114 
00115 FCL_REAL kIOS::height() const
00116 {
00117   return obb.height();
00118 }
00119   
00120 FCL_REAL kIOS::depth() const
00121 {
00122   return obb.depth();
00123 }
00124 
00125 FCL_REAL kIOS::volume() const
00126 {
00127   return obb.volume();
00128 }
00129 
00130 FCL_REAL kIOS::size() const
00131 {
00132   return volume();
00133 }
00134 
00135 FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
00136 {
00137   FCL_REAL d_max = 0;
00138   unsigned int id_a = -1, id_b = -1;
00139   for(unsigned int i = 0; i < num_spheres; ++i)
00140   {
00141     for(unsigned int j = 0; j < other.num_spheres; ++j)
00142     {
00143       FCL_REAL d = (spheres[i].o - other.spheres[j].o).length() - (spheres[i].r + other.spheres[j].r);
00144       if(d_max < d)
00145       {
00146         d_max = d;
00147         if(P && Q)
00148         {
00149           id_a = i; id_b = j;
00150         }
00151       }
00152     }
00153   }
00154 
00155   if(P && Q)
00156   {
00157     if(id_a != -1 && id_b != -1)
00158     {
00159       Vec3f v = spheres[id_a].o - spheres[id_b].o;
00160       FCL_REAL len_v = v.length();
00161       *P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
00162       *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
00163     }
00164   }
00165 
00166   return d_max;
00167 }
00168 
00169   
00170 bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2)
00171 {
00172   kIOS b2_temp = b2;
00173   for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
00174   {
00175     b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
00176   }
00177 
00178   
00179   b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
00180   b2_temp.obb.axis[0] = R0 * b2_temp.obb.axis[0];
00181   b2_temp.obb.axis[1] = R0 * b2_temp.obb.axis[1];
00182   b2_temp.obb.axis[2] = R0 * b2_temp.obb.axis[2];
00183 
00184   return b1.overlap(b2_temp);
00185 }
00186 
00187 FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
00188 {
00189   kIOS b2_temp = b2;
00190   for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
00191   {
00192     b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
00193   }
00194 
00195   return b1.distance(b2_temp, P, Q);
00196 }
00197 
00198 
00199 kIOS translate(const kIOS& bv, const Vec3f& t)
00200 {
00201   kIOS res(bv);
00202   for(size_t i = 0; i < res.num_spheres; ++i)
00203   {
00204     res.spheres[i].o += t;
00205   }
00206   
00207   translate(res.obb, t);
00208   return res;
00209 }
00210 
00211 
00212 }
 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