traversal_node_shapes.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 
38 #ifndef HPP_FCL_TRAVERSAL_NODE_SHAPES_H
39 #define HPP_FCL_TRAVERSAL_NODE_SHAPES_H
40 
42 
43 #include <hpp/fcl/collision_data.h>
45 #include <hpp/fcl/BV/BV.h>
48 
49 namespace hpp {
50 namespace fcl {
51 
54 
56 template <typename S1, typename S2>
57 class HPP_FCL_DLLAPI ShapeCollisionTraversalNode
58  : public CollisionTraversalNodeBase {
59  public:
60  ShapeCollisionTraversalNode(const CollisionRequest& request)
61  : CollisionTraversalNodeBase(request) {
62  model1 = NULL;
63  model2 = NULL;
64 
65  nsolver = NULL;
66  }
67 
69  bool BVDisjoints(int, int, FCL_REAL&) const {
70  HPP_FCL_THROW_PRETTY("Not implemented", std::runtime_error);
71  }
72 
74  void leafCollides(int, int, FCL_REAL&) const {
76  if (request.enable_contact &&
77  request.num_max_contacts > result->numContacts()) {
78  Vec3f contact_point, normal;
79  if (nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true,
80  &contact_point, &normal)) {
81  result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE,
82  contact_point, normal, distance));
83  }
84  } else {
85  bool res = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance,
86  request.enable_distance_lower_bound,
87  NULL, NULL);
88  if (request.enable_distance_lower_bound)
89  result->updateDistanceLowerBound(distance);
90  if (res) {
91  if (request.num_max_contacts > result->numContacts())
92  result->addContact(
93  Contact(model1, model2, Contact::NONE, Contact::NONE));
94  }
95  }
96  }
97 
98  const S1* model1;
99  const S2* model2;
100 
101  const GJKSolver* nsolver;
102 };
103 
105 
108 
110 template <typename S1, typename S2>
111 class HPP_FCL_DLLAPI ShapeDistanceTraversalNode
112  : public DistanceTraversalNodeBase {
113  public:
114  ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
115  model1 = NULL;
116  model2 = NULL;
117 
118  nsolver = NULL;
119  }
120 
122  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const {
123  return -1; // should not be used
124  }
125 
127  void leafComputeDistance(unsigned int, unsigned int) const {
129  Vec3f closest_p1, closest_p2, normal;
130  nsolver->shapeDistance(*model1, tf1, *model2, tf2, distance, closest_p1,
131  closest_p2, normal);
132  result->update(distance, model1, model2, DistanceResult::NONE,
133  DistanceResult::NONE, closest_p1, closest_p2, normal);
134  }
135 
136  const S1* model1;
137  const S2* model2;
138 
139  const GJKSolver* nsolver;
140 };
141 
143 
144 } // namespace fcl
145 
146 } // namespace hpp
147 
149 
150 #endif
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
traversal_node_base.h
geometric_shapes_utility.h
narrowphase.h
res
res
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
collision_data.h
BV.h
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: include/hpp/fcl/fwd.hh:57
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:83
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:451


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:15