hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
45#include <hpp/fcl/BV/BV.h>
48
49namespace hpp
50{
51namespace fcl
52{
53
56
58template<typename S1, typename S2>
59class HPP_FCL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
60{
61public:
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;
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
128template<typename S1, typename S2>
129class HPP_FCL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBase
130{
131public:
132 ShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
133 {
134 model1 = NULL;
135 model2 = NULL;
136
137 nsolver = NULL;
138 }
139
141 FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const
142 {
143 return -1; // should not be used
144 }
145
147 void leafComputeDistance(unsigned int, unsigned int) const
148 {
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
#define HPP_FCL_DLLAPI
Definition: config.hh:64
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.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44