38#ifndef HPP_FCL_TRAVERSAL_NODE_SHAPES_H
39#define HPP_FCL_TRAVERSAL_NODE_SHAPES_H
58template<
typename S1,
typename S2>
59class HPP_FCL_DLLAPI ShapeCollisionTraversalNode :
public CollisionTraversalNodeBase
62 ShapeCollisionTraversalNode(
const CollisionRequest& request) :
63 CollisionTraversalNodeBase(request)
72 bool BVDisjoints(
int,
int)
const
78 bool BVDisjoints(
int,
int, FCL_REAL&)
const
80 throw std::runtime_error (
"Not implemented");
84 void leafCollides(
int,
int, FCL_REAL&)
const
86 bool is_collision =
false;
88 if(request.enable_contact && request.num_max_contacts > result->numContacts())
90 Vec3f contact_point, normal;
91 if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance,
true,
92 &contact_point, &normal))
95 result->addContact(Contact(model1, model2, Contact::NONE,
96 Contact::NONE, contact_point,
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);
109 if(request.num_max_contacts > result->numContacts())
110 result->addContact(Contact(model1, model2, Contact::NONE,
119 const GJKSolver* nsolver;
128template<
typename S1,
typename S2>
129class HPP_FCL_DLLAPI ShapeDistanceTraversalNode :
public DistanceTraversalNodeBase
132 ShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
141 FCL_REAL BVDistanceLowerBound(
unsigned int,
unsigned int)
const
147 void leafComputeDistance(
unsigned int,
unsigned int)
const
150 Vec3f closest_p1, closest_p2, normal;
151 nsolver->shapeDistance(*model1, tf1, *model2, tf2, distance, closest_p1,
153 result->update(distance, model1, model2, DistanceResult::NONE,
154 DistanceResult::NONE, closest_p1, closest_p2, normal);
160 const GJKSolver* nsolver;
#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