collision_node-inl.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-2016, 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 
38 #ifndef FCL_COLLISION_NODE_INL_H
39 #define FCL_COLLISION_NODE_INL_H
40 
42 
44 namespace fcl
45 {
46 
47 namespace detail
48 {
49 
50 //==============================================================================
51 extern template
53 
54 //==============================================================================
55 extern template
57 
58 //==============================================================================
59 extern template
60 void distance(DistanceTraversalNodeBase<double>* node, BVHFrontList* front_list, int qsize);
61 
62 //==============================================================================
63 extern template
65 
66 //==============================================================================
67 extern template
69 
70 //==============================================================================
71 template <typename S>
73 {
74  if(front_list && front_list->size() > 0)
75  {
76  propagateBVHFrontListCollisionRecurse(node, front_list);
77  }
78  else
79  {
80  collisionRecurse(node, 0, 0, front_list);
81  }
82 }
83 
84 //==============================================================================
85 template <typename S>
87 {
88  if(front_list && front_list->size() > 0)
89  {
90  propagateBVHFrontListCollisionRecurse(node, front_list);
91  }
92  else
93  {
94  Matrix3<S> Rtemp, R;
95  Vector3<S> Ttemp, T;
96  Rtemp = node->R * node->model2->getBV(0).getOrientation();
97  R = node->model1->getBV(0).getOrientation().transpose() * Rtemp;
98  Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T;
99  Ttemp -= node->model1->getBV(0).getCenter();
100  T = node->model1->getBV(0).getOrientation().transpose() * Ttemp;
101 
102  collisionRecurse(node, 0, 0, R, T, front_list);
103  }
104 }
105 
106 //==============================================================================
107 template <typename S>
109 {
110  if(front_list && front_list->size() > 0)
111  {
112  propagateBVHFrontListCollisionRecurse(node, front_list);
113  }
114  else
115  {
116  collisionRecurse(node, 0, 0, node->R, node->T, front_list);
117  }
118 }
119 
120 //==============================================================================
121 template <typename S>
123 {
124 
125  if(front_list && front_list->size() > 0)
126  {
127  propagateBVHFrontListCollisionRecurse(node, front_list);
128  }
129  else
130  {
131  selfCollisionRecurse(node, 0, front_list);
132  }
133 }
134 
135 //==============================================================================
136 template <typename S>
137 void distance(DistanceTraversalNodeBase<S>* node, BVHFrontList* front_list, int qsize)
138 {
139  node->preprocess();
140 
141  if(qsize <= 2)
142  distanceRecurse(node, 0, 0, front_list);
143  else
144  distanceQueueRecurse(node, 0, 0, front_list, qsize);
145 
146  node->postprocess();
147 }
148 
149 } // namespace detail
150 } // namespace fcl
151 
152 #endif
fcl::detail::MeshCollisionTraversalNodeOBB::T
Vector3< S > T
Definition: mesh_collision_traversal_node.h:117
fcl::detail::TraversalNodeBase::preprocess
virtual void preprocess()
Definition: traversal_node_base-inl.h:64
fcl::BVHModel::getBV
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model-inl.h:162
fcl::detail::DistanceTraversalNodeBase
Node structure encoding the information required for distance traversal.
Definition: distance_traversal_node_base.h:53
fcl::detail::DistanceTraversalNodeBase< double >
template class FCL_EXPORT DistanceTraversalNodeBase< double >
fcl::detail::MeshCollisionTraversalNodeRSS::R
Matrix3< S > R
Definition: mesh_collision_traversal_node.h:158
fcl::detail::MeshCollisionTraversalNodeRSS< double >
template class FCL_EXPORT MeshCollisionTraversalNodeRSS< double >
fcl::detail::selfCollide
template void selfCollide(CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list)
fcl::detail::MeshCollisionTraversalNodeOBB< double >
template class FCL_EXPORT MeshCollisionTraversalNodeOBB< double >
fcl::detail::BVHCollisionTraversalNode< OBB< S > >::model2
const BVHModel< OBB< S > > * model2
The second BVH model.
Definition: bvh_collision_traversal_node.h:89
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::detail::selfCollisionRecurse
template void selfCollisionRecurse(CollisionTraversalNodeBase< double > *node, int b, BVHFrontList *front_list)
fcl::BVNode::getCenter
Vector3< S > getCenter() const
Access the center of the BV.
Definition: BV_node-inl.h:63
fcl::detail::TraversalNodeBase::postprocess
virtual void postprocess()
Definition: traversal_node_base-inl.h:71
fcl::detail::BVHCollisionTraversalNode< OBB< S > >::model1
const BVHModel< OBB< S > > * model1
The first BVH model.
Definition: bvh_collision_traversal_node.h:86
fcl::detail::CollisionTraversalNodeBase
Node structure encoding the information required for collision traversal.
Definition: collision_traversal_node_base.h:52
fcl::detail::collide
template void collide(CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list)
fcl::detail::CollisionTraversalNodeBase< double >
template class FCL_EXPORT CollisionTraversalNodeBase< double >
fcl::detail::MeshCollisionTraversalNodeOBB::R
Matrix3< S > R
Definition: mesh_collision_traversal_node.h:116
fcl::detail::propagateBVHFrontListCollisionRecurse
template void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list)
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
fcl::detail::distanceRecurse
template void distanceRecurse(DistanceTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list)
fcl::detail::distanceQueueRecurse
template void distanceQueueRecurse(DistanceTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list, int qsize)
collision_node.h
fcl::detail::BVHFrontList
std::list< BVHFrontNode > BVHFrontList
BVH front list is a list of front nodes.
Definition: BVH_front.h:69
fcl::detail::MeshCollisionTraversalNodeRSS::T
Vector3< S > T
Definition: mesh_collision_traversal_node.h:159
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::MeshCollisionTraversalNodeRSS
Definition: mesh_collision_traversal_node.h:139
fcl::detail::MeshCollisionTraversalNodeOBB
Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB,...
Definition: mesh_collision_traversal_node.h:99
fcl::BVNode::getOrientation
Matrix3< S > getOrientation() const
Access the orientation of the BV.
Definition: BV_node-inl.h:80
fcl::detail::collisionRecurse
template void collisionRecurse(CollisionTraversalNodeBase< double > *node, int b1, int b2, BVHFrontList *front_list)
fcl::detail::collide2
template void collide2(MeshCollisionTraversalNodeOBB< double > *node, BVHFrontList *front_list)


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:37:59