bullet_discrete_bvh_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD-2-Clause)
3  *
4  * Copyright (c) 2017, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  *********************************************************************/
31 
32 /* Author: Levi Armstrong, Jens Petit */
33 
35 
37 {
38 BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const
39 {
40  BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager());
41 
42  for (const std::pair<const std::string, CollisionObjectWrapperPtr>& cow : link2cow_)
43  {
44  CollisionObjectWrapperPtr new_cow = cow.second->clone();
45 
46  assert(new_cow->getCollisionShape());
47  assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
48 
49  new_cow->setWorldTransform(cow.second->getWorldTransform());
50  new_cow->setContactProcessingThreshold(static_cast<btScalar>(contact_distance_));
51  manager->addCollisionObject(new_cow);
52  }
53 
54  manager->setActiveCollisionObjects(active_);
55  manager->setContactDistanceThreshold(contact_distance_);
56 
57  return manager;
58 }
59 
62  const collision_detection::AllowedCollisionMatrix* acm, bool self)
63 {
64  ContactTestData cdata(active_, contact_distance_, collisions, req);
65 
66  broadphase_->calculateOverlappingPairs(dispatcher_.get());
67  btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache();
68 
69  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
70  "Num overlapping candidates " << pair_cache->getNumOverlappingPairs());
71 
73  TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc);
74  pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get());
75 
76  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet", (collisions.collision ? "In" : "No")
77  << " collision with " << collisions.contact_count
78  << " collisions");
79 }
80 
81 void BulletDiscreteBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow)
82 {
83  link2cow_[cow->getName()] = cow;
85 }
86 
87 } // namespace collision_detection_bullet
collision_detection_bullet
Definition: basic_types.h:34
collision_detection_bullet::BulletDiscreteBVHManager::clone
BulletDiscreteBVHManagerPtr clone() const
Clone the manager.
Definition: bullet_discrete_bvh_manager.cpp:67
collision_detection_bullet::BulletBVHManager::dispatcher_
std::unique_ptr< btCollisionDispatcher > dispatcher_
The bullet collision dispatcher used for getting object to object collison algorithm.
Definition: bullet_bvh_manager.h:172
collision_detection_bullet::BulletBVHManager::broadphase_
std::unique_ptr< btBroadphaseInterface > broadphase_
The bullet broadphase interface.
Definition: bullet_bvh_manager.h:181
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
collision_detection_bullet::BulletDiscreteBVHManager::contactTest
void contactTest(collision_detection::CollisionResult &collisions, const collision_detection::CollisionRequest &req, const collision_detection::AllowedCollisionMatrix *acm, bool self) override
Perform a contact test for all objects in the manager.
Definition: bullet_discrete_bvh_manager.cpp:89
collision_detection_bullet::BulletBVHManager::active_
std::vector< std::string > active_
A list of the active collision links.
Definition: bullet_bvh_manager.h:166
collision_detection_bullet::addCollisionObjectToBroadphase
void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Add the collision object to broadphase.
Definition: bullet_utils.h:949
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
collision_detection_bullet::TesseractCollisionPairCallback
A callback function that is called as part of the broadphase collision checking.
Definition: bullet_utils.h:720
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_detection_bullet::BulletBVHManager::link2cow_
std::map< std::string, CollisionObjectWrapperPtr > link2cow_
A map of collision objects being managed.
Definition: bullet_bvh_manager.h:163
collision_detection_bullet::BroadphaseContactResultCallback
Callback structure for both discrete and continuous broadphase collision pair.
Definition: bullet_utils.h:597
bullet_discrete_bvh_manager.h
collision_detection_bullet::BulletDiscreteBVHManager::addCollisionObject
void addCollisionObject(const CollisionObjectWrapperPtr &cow) override
Add a bullet collision object to the manager.
Definition: bullet_discrete_bvh_manager.cpp:110
collision_detection_bullet::BulletBVHManager::dispatch_info_
btDispatcherInfo dispatch_info_
The bullet collision dispatcher configuration information.
Definition: bullet_bvh_manager.h:175
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
collision_detection_bullet::BulletBVHManager::contact_distance_
double contact_distance_
The contact distance threshold.
Definition: bullet_bvh_manager.h:169
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:206
collision_detection_bullet::BulletDiscreteBVHManager::BulletDiscreteBVHManager
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BulletDiscreteBVHManager()=default
Constructor.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun May 15 2022 02:24:36