hpp-fcl: include/hpp/fcl/internal/traversal_node_shapes.h Source File

hpp-fcl 1.7.0 HPP fork of FCL -- The Flexible Collision Library

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>
44 #include <hpp/fcl/narrowphase/narrowphase.h>
45 #include <hpp/fcl/BV/BV.h>
46 #include <hpp/fcl/shape/geometric_shapes_utility.h>
47 #include <hpp/fcl/internal/traversal_node_base.h>
48
49 namespace hpp
50 {
51 namespace fcl
52 {
53
56
58 template<typename S1, typename S2>
59 class HPP_FCL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
60 {
61 public:
62 ShapeCollisionTraversalNode(const CollisionRequest& request) :
63 CollisionTraversalNodeBase(request)
64 {
65 model1 = NULL;
66 model2 = NULL;
67
68 nsolver = NULL;
69 }
70
72 bool BVDisjoints(int, int) const
73 {
74 return false;
75 }
76
78 bool BVDisjoints(int, int, FCL_REAL&) const
79 {
80 throw std::runtime_error ("Not implemented");
81 }
82
84 void leafCollides(int, int, FCL_REAL&) const
85 {
86 bool is_collision = false;
87 FCL_REAL distance;
88 if(request.enable_contact && request.num_max_contacts > result->numContacts())
89 {
90 Vec3f contact_point, normal;
91 if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true,
92 &contact_point, &normal))
93 {
94 is_collision = true;
95 result->addContact(Contact(model1, model2, Contact::NONE,
96 Contact::NONE, contact_point,
97 normal, distance));
98 }
99 }
100 else
101 {
102 bool res = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance,
103 request.enable_distance_lower_bound, NULL, NULL);
104 if (request.enable_distance_lower_bound)
105 result->updateDistanceLowerBound (distance);
106 if(res)
107 {
108 is_collision = true;
109 if(request.num_max_contacts > result->numContacts())
110 result->addContact(Contact(model1, model2, Contact::NONE,
111 Contact::NONE));
112 }
113 }
114 }
115
116 const S1* model1;
117 const S2* model2;
118
119 const GJKSolver* nsolver;
120 };
121
123
126
128 template<typename S1, typename S2>
129 class HPP_FCL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBase
130 {
131 public:
132 ShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
133 {
134 model1 = NULL;
135 model2 = NULL;
136
137 nsolver = NULL;
138 }
139
141 FCL_REAL BVDistanceLowerBound(int, int) const
142 {
143 return -1; // should not be used
144 }
145
147 void leafComputeDistance(int, int) const
148 {
149 FCL_REAL distance;
150 Vec3f closest_p1, closest_p2, normal;
151 nsolver->shapeDistance(*model1, tf1, *model2, tf2, distance, closest_p1,
152 closest_p2, normal);
153 result->update(distance, model1, model2, DistanceResult::NONE,
154 DistanceResult::NONE, closest_p1, closest_p2, normal);
155 }
156
157 const S1* model1;
158 const S2* model2;
159
160 const GJKSolver* nsolver;
161 };
162
164
165 }
166
167 } // namespace hpp
168
170
171 #endif
BV.h
collision_data.h
geometric_shapes_utility.h
hpp::fcl::distance
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.
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
narrowphase.h
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:87
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:423
traversal_node_base.h