fcl » include » fcl » BV
BV_node.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 
00038 #ifndef FCL_BV_NODE_H
00039 #define FCL_BV_NODE_H
00040 
00041 #include "fcl/math/vec_3f.h"
00042 #include "fcl/math/matrix_3f.h"
00043 
00044 #include "fcl/BV/BV.h"
00045 #include <iostream>
00046 
00047 namespace fcl
00048 {
00049 
00051 struct BVNodeBase
00052 {
00057   int first_child;
00058 
00061   int first_primitive;
00062 
00064   int num_primitives;
00065 
00067   inline bool isLeaf() const { return first_child < 0; }
00068 
00070   inline int primitiveId() const { return -(first_child + 1); }
00071 
00073   inline int leftChild() const { return first_child; }
00074 
00076   inline int rightChild() const { return first_child + 1; }
00077 };
00078 
00080 template<typename BV>
00081 struct BVNode : public BVNodeBase
00082 {
00084   BV bv;
00085 
00087   bool overlap(const BVNode& other) const
00088   {
00089     return bv.overlap(other.bv);
00090   }
00091 
00093   FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
00094   {
00095     return bv.distance(other.bv, P1, P2);
00096   }
00097 
00099   Vec3f getCenter() const { return bv.center(); }
00100 
00102   Matrix3f getOrientation() const { return Matrix3f::getIdentity(); }
00103 };
00104 
00105 template<>
00106 inline Matrix3f BVNode<OBB>::getOrientation() const 
00107 {
00108   return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00109                   bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00110                   bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
00111 }
00112 
00113 template<>
00114 inline Matrix3f BVNode<RSS>::getOrientation() const 
00115 {
00116   return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
00117                   bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
00118                   bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
00119 }
00120 
00121 template<>
00122 inline Matrix3f BVNode<OBBRSS>::getOrientation() const 
00123 {
00124   return Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
00125                   bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
00126                   bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]);
00127 }
00128 
00129 
00130 }
00131 
00132 #endif