hpp-fcl  1.4.4
HPP fork of FCL -- The Flexible Collision Library
BV_node.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
39 #ifndef HPP_FCL_BV_NODE_H
40 #define HPP_FCL_BV_NODE_H
41 
42 #include <hpp/fcl/data_types.h>
43 
44 #include <hpp/fcl/BV/BV.h>
45 #include <iostream>
46 
47 namespace hpp
48 {
49 namespace fcl
50 {
51 
55 
57 struct BVNodeBase
58 {
64 
68 
71 
73  inline bool isLeaf() const { return first_child < 0; }
74 
76  inline int primitiveId() const { return -(first_child + 1); }
77 
79  inline int leftChild() const { return first_child; }
80 
82  inline int rightChild() const { return first_child + 1; }
83 };
84 
86 template<typename BV>
87 struct BVNode : public BVNodeBase
88 {
90  BV bv;
91 
93  bool overlap(const BVNode& other) const
94  {
95  return bv.overlap(other.bv);
96  }
98  bool overlap(const BVNode& other, const CollisionRequest& request,
99  FCL_REAL& sqrDistLowerBound) const
100  {
101  return bv.overlap(other.bv, request, sqrDistLowerBound);
102  }
103 
105  FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
106  {
107  return bv.distance(other.bv, P1, P2);
108  }
109 
111  Vec3f getCenter() const { return bv.center(); }
112 
114  const Matrix3f& getOrientation() const
115  {
116  static const Matrix3f id3 = Matrix3f::Identity();
117  return id3;
118  }
119 
121  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
123 };
124 
125 template<>
126 inline const Matrix3f& BVNode<OBB>::getOrientation() const
127 {
128  return bv.axes;
129 }
130 
131 template<>
132 inline const Matrix3f& BVNode<RSS>::getOrientation() const
133 {
134  return bv.axes;
135 }
136 
137 template<>
139 {
140  return bv.obb.axes;
141 }
142 
143 
144 }
145 
146 } // namespace hpp
147 
148 #endif
bool overlap(const BVNode &other) const
Check whether two BVNode collide.
Definition: BV_node.h:93
Main namespace.
Definition: AABB.h:43
BV bv
bounding volume storing the geometry
Definition: BV_node.h:90
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
Vec3f getCenter() const
Access to the center of the BV.
Definition: BV_node.h:111
int primitiveId() const
Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices)...
Definition: BV_node.h:76
int first_primitive
The start id the primitive belonging to the current node. The index is referred to the primitive_indi...
Definition: BV_node.h:67
int first_child
An index for first child node or primitive If the value is positive, it is the index of the first chi...
Definition: BV_node.h:63
request to the collision algorithm
Definition: collision_data.h:150
double FCL_REAL
Definition: data_types.h:68
bool overlap(const BVNode &other, const CollisionRequest &request, FCL_REAL &sqrDistLowerBound) const
Check whether two BVNode collide.
Definition: BV_node.h:98
const Matrix3f & getOrientation() const
Access to the orientation of the BV.
Definition: BV_node.h:114
FCL_REAL distance(const BVNode &other, Vec3f *P1=NULL, Vec3f *P2=NULL) const
Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distan...
Definition: BV_node.h:105
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:87
int leftChild() const
Return the index of the first child. The index is referred to the bounding volume array (i...
Definition: BV_node.h:79
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index.
Definition: BV_node.h:73
int num_primitives
The number of primitives belonging to the current node.
Definition: BV_node.h:70
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
int rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i...
Definition: BV_node.h:82
BVNodeBase encodes the tree structure for BVH.
Definition: BV_node.h:57