fcl » src » broadphase
hierarchy_tree.cpp

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/broadphase/hierarchy_tree.h"
00038 
00039 namespace fcl
00040 {
00041 
00042 template<>
00043 size_t select(const NodeBase<AABB>& node, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2)
00044 {
00045   const AABB& bv = node.bv;
00046   const AABB& bv1 = node1.bv;
00047   const AABB& bv2 = node2.bv;
00048   Vec3f v = bv.min_ + bv.max_;
00049   Vec3f v1 = v - (bv1.min_ + bv1.max_);
00050   Vec3f v2 = v - (bv2.min_ + bv2.max_);
00051   FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
00052   FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
00053   return (d1 < d2) ? 0 : 1;
00054 }
00055 
00056 template<>
00057 size_t select(const AABB& query, const NodeBase<AABB>& node1, const NodeBase<AABB>& node2)
00058 {
00059   const AABB& bv = query;
00060   const AABB& bv1 = node1.bv;
00061   const AABB& bv2 = node2.bv;
00062   Vec3f v = bv.min_ + bv.max_;
00063   Vec3f v1 = v - (bv1.min_ + bv1.max_);
00064   Vec3f v2 = v - (bv2.min_ + bv2.max_);
00065   FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
00066   FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
00067   return (d1 < d2) ? 0 : 1;
00068 }
00069 
00070 template<>
00071 bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin)
00072 {
00073   AABB bv(bv_);
00074   if(leaf->bv.contain(bv)) return false;
00075   Vec3f marginv(margin);
00076   bv.min_ -= marginv;
00077   bv.max_ += marginv;
00078   if(vel[0] > 0) bv.max_[0] += vel[0];
00079   else bv.min_[0] += vel[0];
00080   if(vel[1] > 0) bv.max_[1] += vel[1];
00081   else bv.min_[1] += vel[1];
00082   if(vel[2] > 0) bv.max_[2] += vel[2];
00083   else bv.max_[2] += vel[2];
00084   update(leaf, bv);
00085   return true;
00086 }
00087 
00088 template<>
00089 bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel)
00090 {
00091   AABB bv(bv_);
00092   if(leaf->bv.contain(bv)) return false;
00093   if(vel[0] > 0) bv.max_[0] += vel[0];
00094   else bv.min_[0] += vel[0];
00095   if(vel[1] > 0) bv.max_[1] += vel[1];
00096   else bv.min_[1] += vel[1];
00097   if(vel[2] > 0) bv.max_[2] += vel[2];
00098   else bv.max_[2] += vel[2];
00099   update(leaf, bv);
00100   return true;
00101 }
00102 
00103 namespace implementation_array
00104 {
00105 template<>
00106 size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
00107 {
00108   const AABB& bv = nodes[query].bv;
00109   const AABB& bv1 = nodes[node1].bv;
00110   const AABB& bv2 = nodes[node2].bv;
00111   Vec3f v = bv.min_ + bv.max_;
00112   Vec3f v1 = v - (bv1.min_ + bv1.max_);
00113   Vec3f v2 = v - (bv2.min_ + bv2.max_);
00114   FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
00115   FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
00116   return (d1 < d2) ? 0 : 1;
00117 }
00118 
00119 template<>
00120 size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
00121 {
00122   const AABB& bv = query;
00123   const AABB& bv1 = nodes[node1].bv;
00124   const AABB& bv2 = nodes[node2].bv;
00125   Vec3f v = bv.min_ + bv.max_;
00126   Vec3f v1 = v - (bv1.min_ + bv1.max_);
00127   Vec3f v2 = v - (bv2.min_ + bv2.max_);
00128   FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
00129   FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
00130   return (d1 < d2) ? 0 : 1;
00131 }
00132 
00133 }
00134 
00135 }