Make ConstraintSolver to return CollisionDetector/CollisionGroup as s…

…hared pointers
dartsim · Apr 22, 2016 · bf8cdb9 · bf8cdb9
1 parent d0563b3
commit bf8cdb9
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 24 deletions.
diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
@@ -66,7 +66,7 @@ using namespace dynamics;
 //==============================================================================
 ConstraintSolver::ConstraintSolver(double timeStep)
   : mCollisionDetector(collision::FCLCollisionDetector::create()),
-    mCollisionGroup(mCollisionDetector->createCollisionGroup()),
+    mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
     mCollisionOption(
       collision::CollisionOption(
         true, false, 100, std::make_shared<collision::BodyNodeCollisionFilter>())),
@@ -223,42 +223,47 @@ void ConstraintSolver::setCollisionDetector(
 void ConstraintSolver::setCollisionDetector(
   const std::shared_ptr<collision::CollisionDetector>& collisionDetector)
 {
-  assert(collisionDetector && "Invalid collision detector.");
+  if (!collisionDetector)
+  {
+    dtwarn << "[ConstraintSolver::setCollisionDetector] Attempting to assign "
+           << "nullptr as the new collision detector to the constraint solver, "
+           << "which is not allowed. Ignoring.\n";
+  }
+
+  if (mCollisionDetector == collisionDetector)
+    return;
 
-  // Change the collision detector of the constraint solver to new one
   mCollisionDetector = collisionDetector;
 
-  auto newCollisionGroup = mCollisionDetector->createCollisionGroup();
+  mCollisionGroup = mCollisionDetector->createCollisionGroupAsSharedPtr();
 
   for (const auto& skeleton : mSkeletons)
-    newCollisionGroup->addShapeFramesOf(skeleton.get());
-
-  mCollisionGroup = std::move(newCollisionGroup);
+    mCollisionGroup->addShapeFramesOf(skeleton.get());
 }
 
 //==============================================================================
-collision::CollisionDetector* ConstraintSolver::getCollisionDetector()
+collision::CollisionDetectorPtr ConstraintSolver::getCollisionDetector()
 {
-  return mCollisionDetector.get();
+  return mCollisionDetector;
 }
 
 //==============================================================================
-const collision::CollisionDetector*
+collision::ConstCollisionDetectorPtr
 ConstraintSolver::getCollisionDetector() const
 {
-  return mCollisionDetector.get();
+  return mCollisionDetector;
 }
 
 //==============================================================================
-collision::CollisionGroup* ConstraintSolver::getCollisionGroup()
+collision::CollisionGroupPtr ConstraintSolver::getCollisionGroup()
 {
-  return mCollisionGroup.get();
+  return mCollisionGroup;
 }
 
 //==============================================================================
-const collision::CollisionGroup* ConstraintSolver::getCollisionGroup() const
+collision::ConstCollisionGroupPtr ConstraintSolver::getCollisionGroup() const
 {
-  return mCollisionGroup.get();
+  return mCollisionGroup;
 }
 
 //==============================================================================
@@ -268,7 +273,8 @@ collision::CollisionResult& ConstraintSolver::getLastCollisionResult()
 }
 
 //==============================================================================
-const collision::CollisionResult& ConstraintSolver::getLastCollisionResult() const
+const collision::CollisionResult&
+ConstraintSolver::getLastCollisionResult() const
 {
   return mCollisionResult;
 }

diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h
@@ -114,18 +114,18 @@ class ConstraintSolver
       const std::shared_ptr<collision::CollisionDetector>& collisionDetector);
 
   /// Get collision detector
-  collision::CollisionDetector* getCollisionDetector();
+  collision::CollisionDetectorPtr getCollisionDetector();
 
-  /// Get collision detector
-  const collision::CollisionDetector* getCollisionDetector() const;
+  /// Get (const) collision detector
+  collision::ConstCollisionDetectorPtr getCollisionDetector() const;
 
   /// Return collision group of collision objects that are added to this
   /// ConstraintSolver
-  collision::CollisionGroup* getCollisionGroup();
+  collision::CollisionGroupPtr getCollisionGroup();
 
   /// Return (const) collision group of collision objects that are added to this
   /// ConstraintSolver
-  const collision::CollisionGroup* getCollisionGroup() const;
+  collision::ConstCollisionGroupPtr getCollisionGroup() const;
 
   /// Return the last collision checking result
   collision::CollisionResult& getLastCollisionResult();
@@ -170,12 +170,12 @@ class ConstraintSolver
   using CollisionDetector = collision::CollisionDetector;
 
   /// Collision detector
-  std::shared_ptr<collision::CollisionDetector> mCollisionDetector;
+  collision::CollisionDetectorPtr mCollisionDetector;
 
   /// Collision group
-  std::unique_ptr<collision::CollisionGroup> mCollisionGroup;
+  collision::CollisionGroupPtr mCollisionGroup;
 
-  /// Last collision checking result
+  /// Collision detection option
   collision::CollisionOption mCollisionOption;
 
   /// Last collision checking result