hpp-fcl 2.4.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 {
50namespace fcl {
51
54
56template <typename S1, typename S2>
57class 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
110template <typename S1, typename S2>
111class 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
#define HPP_FCL_DLLAPI
Definition: config.hh:88
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
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:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44