38 #include <rclcpp/logger.hpp>
39 #include <rclcpp/logging.hpp>
41 static const rclcpp::Logger
BULLET_LOGGER = rclcpp::get_logger(
"collision_detection.bullet");
46 const std::pair<std::string, std::string>& key,
bool found)
57 RCLCPP_DEBUG_STREAM(
BULLET_LOGGER,
"Contact btw " << key.first <<
" and " << key.second <<
" dist: " << contact.
depth);
61 if (contact.
depth <= 0)
66 std::vector<collision_detection::Contact> data;
80 data.emplace_back(contact);
97 return &(cdata.
res.
contacts.insert(std::make_pair(key, data)).first->second.back());
101 std::vector<collision_detection::Contact>& dr = cdata.
res.
contacts[key];
102 dr.emplace_back(contact);
130 btConvexHullComputer conv;
131 btAlignedObjectArray<btVector3> points;
132 points.reserve(
static_cast<int>(
input.size()));
135 points.push_back(btVector3(
static_cast<btScalar
>(v[0]),
static_cast<btScalar
>(v[1]),
static_cast<btScalar
>(v[2])));
138 btScalar val = conv.compute(&points[0].getX(),
sizeof(btVector3), points.size(),
static_cast<btScalar
>(shrink),
139 static_cast<btScalar
>(shrinkClamp));
146 int num_verts = conv.vertices.size();
147 vertices.reserve(
static_cast<size_t>(num_verts));
148 for (
int i = 0; i < num_verts; ++i)
150 btVector3& v = conv.vertices[i];
154 int num_faces = conv.faces.size();
155 faces.reserve(
static_cast<size_t>(3 * num_faces));
156 for (
int i = 0; i < num_faces; ++i)
158 std::vector<int> face;
161 const btConvexHullComputer::Edge* source_edge = &(conv.edges[conv.faces[i]]);
162 int a = source_edge->getSourceVertex();
165 int b = source_edge->getTargetVertex();
168 const btConvexHullComputer::Edge* edge = source_edge->getNextEdgeOfFace();
169 int c = edge->getTargetVertex();
172 edge = edge->getNextEdgeOfFace();
173 c = edge->getTargetVertex();
178 edge = edge->getNextEdgeOfFace();
179 c = edge->getTargetVertex();
181 faces.push_back(
static_cast<int>(face.size()));
182 faces.insert(faces.end(), face.begin(), face.end());
collision_detection::Contact * processResult(ContactTestData &cdata, collision_detection::Contact &contact, const std::pair< std::string, std::string > &key, bool found)
Stores a single contact result in the requested way.
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
int createConvexHull(AlignedVector< Eigen::Vector3d > &vertices, std::vector< int > &faces, const AlignedVector< Eigen::Vector3d > &input, double shrink=-1, double shrinkClamp=-1)
Create a convex hull from vertices using Bullet Convex Hull Computer.
Vec3fX< details::Vec3Data< double > > Vector3d
const rclcpp::Logger BULLET_LOGGER
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
bool distance
If true, compute proximity distance.
double distance
Closest distance between two bodies.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.