diff --git a/Examples/CollisionDetection/BoxBoxMeshTest/BoxBoxMeshTestExample.cpp b/Examples/CollisionDetection/BoxVsBoxMeshTest/BoxVsBoxMeshTestExample.cpp
similarity index 100%
rename from Examples/CollisionDetection/BoxBoxMeshTest/BoxBoxMeshTestExample.cpp
rename to Examples/CollisionDetection/BoxVsBoxMeshTest/BoxVsBoxMeshTestExample.cpp
diff --git a/Examples/CollisionDetection/BoxBoxMeshTest/CMakeLists.txt b/Examples/CollisionDetection/BoxVsBoxMeshTest/CMakeLists.txt
similarity index 93%
rename from Examples/CollisionDetection/BoxBoxMeshTest/CMakeLists.txt
rename to Examples/CollisionDetection/BoxVsBoxMeshTest/CMakeLists.txt
index 515eb40360b6311cba6a86595a5bd764df4e0b9e..3fa68fc43fd1b8f9eb5f8e1f4a3d683a1869c6f0 100644
--- a/Examples/CollisionDetection/BoxBoxMeshTest/CMakeLists.txt
+++ b/Examples/CollisionDetection/BoxVsBoxMeshTest/CMakeLists.txt
@@ -16,12 +16,12 @@
 #
 ###########################################################################
 
-project(Example-BoxBoxMeshTest)
+project(Example-BoxVsBoxMeshTest)
 
 #-----------------------------------------------------------------------------
 # Create executable
 #-----------------------------------------------------------------------------
-imstk_add_executable(${PROJECT_NAME} BoxBoxMeshTestExample.cpp)
+imstk_add_executable(${PROJECT_NAME} BoxVsBoxMeshTestExample.cpp)
 
 #-----------------------------------------------------------------------------
 # Add the target to Examples folder
diff --git a/Examples/CollisionDetection/PlaneVsBoxTest/CMakeLists.txt b/Examples/CollisionDetection/PlaneVsBoxTest/CMakeLists.txt
deleted file mode 100644
index 84e9618e58af0355d2a268fd2550051e66b19a71..0000000000000000000000000000000000000000
--- a/Examples/CollisionDetection/PlaneVsBoxTest/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-###########################################################################
-#
-# Copyright (c) Kitware, Inc.
-#
-#  Licensed under the Apache License, Version 2.0 (the "License");
-#  you may not use this file except in compliance with the License.
-#  You may obtain a copy of the License at
-#
-#      http://www.apache.org/licenses/LICENSE-2.0.txt
-#
-#  Unless required by applicable law or agreed to in writing, software
-#  distributed under the License is distributed on an "AS IS" BASIS,
-#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-#  See the License for the specific language governing permissions and
-#  limitations under the License.
-#
-###########################################################################
-
-project(Example-PlaneVsBoxTest)
-
-#-----------------------------------------------------------------------------
-# Create executable
-#-----------------------------------------------------------------------------
-imstk_add_executable(${PROJECT_NAME} PlaneVsBoxTestExample.cpp)
-
-#-----------------------------------------------------------------------------
-# Add the target to Examples folder
-#-----------------------------------------------------------------------------
-SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/CollisionDetection)
-
-#-----------------------------------------------------------------------------
-# Link libraries to executable
-#-----------------------------------------------------------------------------
-target_link_libraries(${PROJECT_NAME} SimulationManager)
diff --git a/Examples/CollisionDetection/PlaneVsBoxTest/PlaneVsBoxTestExample.cpp b/Examples/CollisionDetection/PlaneVsBoxTest/PlaneVsBoxTestExample.cpp
deleted file mode 100644
index e9ea9305c9edb3072fe328fd8dde45123bb9abff..0000000000000000000000000000000000000000
--- a/Examples/CollisionDetection/PlaneVsBoxTest/PlaneVsBoxTestExample.cpp
+++ /dev/null
@@ -1,163 +0,0 @@
-/*=========================================================================
-
-   Library: iMSTK
-
-   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
-   & Imaging in Medicine, Rensselaer Polytechnic Institute.
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-      http://www.apache.org/licenses/LICENSE-2.0.txt
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.
-
-=========================================================================*/
-
-#include "imstkCamera.h"
-#include "imstkCollidingObject.h"
-#include "imstkCollisionDataDebugObject.h"
-#include "imstkDirectionalLight.h"
-#include "imstkGeometryUtilities.h"
-#include "imstkKeyboardDeviceClient.h"
-#include "imstkKeyboardSceneControl.h"
-#include "imstkLight.h"
-#include "imstkMouseSceneControl.h"
-#include "imstkNew.h"
-#include "imstkOrientedBox.h"
-#include "imstkPlane.h"
-#include "imstkRenderMaterial.h"
-#include "imstkScene.h"
-#include "imstkSceneManager.h"
-#include "imstkSimulationManager.h"
-#include "imstkSurfaceMesh.h"
-#include "imstkVisualModel.h"
-#include "imstkVTKRenderer.h"
-#include "imstkVTKViewer.h"
-#include "imstkSurfaceMeshToSurfaceMeshCD.h"
-
-using namespace imstk;
-
-int
-main()
-{
-    // Setup logger (write to file and stdout)
-    Logger::startLogger();
-
-    // Setup the scene
-    imstkNew<Scene> scene("PbdCollisionOneDragon");
-
-    scene->getActiveCamera()->setPosition(0, 3.0, 20.0);
-    scene->getActiveCamera()->setFocalPoint(0.0, -10.0, 0.0);
-
-    imstkNew<CollidingObject> obj1("obj1");
-    imstkNew<OrientedBox>     cube(Vec3d(0.0, 0.0, 0.0), Vec3d(0.5, 0.5, 0.5), Quatd(Rotd(1.0, Vec3d(0.0, 0.0, 1.0))));
-    auto                      surfMesh1 = GeometryUtils::toSurfaceMesh(cube);
-    obj1->setVisualGeometry(surfMesh1);
-    obj1->setCollidingGeometry(surfMesh1);
-    obj1->getVisualModel(0)->getRenderMaterial()->setBackFaceCulling(false);
-    obj1->getVisualModel(0)->getRenderMaterial()->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
-    obj1->getVisualModel(0)->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::Flat);
-    scene->addSceneObject(obj1);
-
-    imstkNew<CollidingObject> obj2("obj2");
-    imstkNew<Plane>           plane(Vec3d(0.0, 0.0, 0.0), Vec3d(0.0, 1.0, 0.0));
-    plane->setWidth(2.0);
-    auto surfMesh2 = GeometryUtils::toSurfaceMesh(plane);
-    obj2->setVisualGeometry(surfMesh2);
-    obj2->setCollidingGeometry(surfMesh2);
-    obj2->getVisualModel(0)->getRenderMaterial()->setBackFaceCulling(false);
-    scene->addSceneObject(obj2);
-
-    imstkNew<SurfaceMeshToSurfaceMeshCD> cd;
-    cd->setInputGeometryA(surfMesh1);
-    cd->setInputGeometryB(surfMesh2);
-    cd->update();
-
-    // Light
-    imstkNew<DirectionalLight> light;
-    light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
-    light->setIntensity(1.0);
-    scene->addLight("Light", light);
-
-    imstkNew<CollisionDataDebugObject> cdDebugObj;
-    cdDebugObj->setInputCD(cd->getCollisionData());
-    cdDebugObj->setPrintContacts(true);
-
-    // Debug geometry
-    scene->addSceneObject(cdDebugObj);
-
-    // Run the simulation
-    {
-        // Setup a viewer to render
-        imstkNew<VTKViewer> viewer("Viewer");
-        viewer->setActiveScene(scene);
-        std::dynamic_pointer_cast<VTKRenderer>(viewer->getActiveRenderer())->setAxesLength(0.05, 0.05, 0.05);
-
-        // Setup a scene manager to advance the scene
-        imstkNew<SceneManager> sceneManager("Scene Manager");
-        sceneManager->setActiveScene(scene);
-
-        sceneManager->init();
-        sceneManager->update();
-
-        //cdDebugObj->debugUpdate();
-
-        connect<KeyEvent>(viewer->getKeyboardDevice(), &KeyboardDeviceClient::keyPress, [&](KeyEvent* e)
-        {
-            const double s = 0.05;
-            if (e->m_key == 'i')
-            {
-                surfMesh1->translate(Vec3d(0.0, 0.0, 1.0) * s);
-            }
-            else if (e->m_key == 'k')
-            {
-                surfMesh1->translate(Vec3d(0.0, 0.0, -1.0) * s);
-            }
-            else if (e->m_key == 'j')
-            {
-                surfMesh1->translate(Vec3d(-1.0, 0.0, 0.0) * s);
-            }
-            else if (e->m_key == 'l')
-            {
-                surfMesh1->translate(Vec3d(1.0, 0.0, 0.0) * s);
-            }
-            else if (e->m_key == 'u')
-            {
-                surfMesh1->translate(Vec3d(0.0, -1.0, 0.0) * s);
-            }
-            else if (e->m_key == 'o')
-            {
-                surfMesh1->translate(Vec3d(0.0, 1.0, 0.0) * s);
-            }
-            surfMesh1->postModified();
-            cd->update();
-            sceneManager->update();
-            cdDebugObj->debugUpdate();
-        });
-
-        imstkNew<SimulationManager> driver;
-        driver->addModule(viewer);
-
-        // Add mouse and keyboard controls to the viewer
-        {
-            imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
-            mouseControl->setSceneManager(sceneManager);
-            viewer->addControl(mouseControl);
-
-            imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
-            keyControl->setSceneManager(sceneManager);
-            keyControl->setModuleDriver(driver);
-            viewer->addControl(keyControl);
-        }
-
-        driver->start();
-    }
-
-    return 0;
-}
diff --git a/Examples/RBD/RbdMeshMeshCollision/CMakeLists.txt b/Examples/RBD/RbdMeshMeshCollision/CMakeLists.txt
deleted file mode 100644
index 11353cdca6176c7b8f1efa22504a642f00e1e482..0000000000000000000000000000000000000000
--- a/Examples/RBD/RbdMeshMeshCollision/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-###########################################################################
-#
-# Copyright (c) Kitware, Inc.
-#
-#  Licensed under the Apache License, Version 2.0 (the "License");
-#  you may not use this file except in compliance with the License.
-#  You may obtain a copy of the License at
-#
-#      http://www.apache.org/licenses/LICENSE-2.0.txt
-#
-#  Unless required by applicable law or agreed to in writing, software
-#  distributed under the License is distributed on an "AS IS" BASIS,
-#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-#  See the License for the specific language governing permissions and
-#  limitations under the License.
-#
-###########################################################################
-
-project(Example-RbdMeshMeshCollision)
-
-#-----------------------------------------------------------------------------
-# Create executable
-#-----------------------------------------------------------------------------
-imstk_add_executable(${PROJECT_NAME} RbdMeshMeshCollisionExample.cpp)
-
-#-----------------------------------------------------------------------------
-# Add the target to Examples folder
-#-----------------------------------------------------------------------------
-SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/RBD)
-
-#-----------------------------------------------------------------------------
-# Link libraries to executable
-#-----------------------------------------------------------------------------
-target_link_libraries(${PROJECT_NAME} SimulationManager Filtering)
diff --git a/Examples/RBD/RbdMeshMeshCollision/RbdMeshMeshCollisionExample.cpp b/Examples/RBD/RbdMeshMeshCollision/RbdMeshMeshCollisionExample.cpp
deleted file mode 100644
index 570e764ed9fef7fdca9ce4c5bdccb4fe5b3e6af5..0000000000000000000000000000000000000000
--- a/Examples/RBD/RbdMeshMeshCollision/RbdMeshMeshCollisionExample.cpp
+++ /dev/null
@@ -1,273 +0,0 @@
-/*=========================================================================
-
-   Library: iMSTK
-
-   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
-   & Imaging in Medicine, Rensselaer Polytechnic Institute.
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-          http://www.apache.org/licenses/LICENSE-2.0.txt
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.
-
-=========================================================================*/
-
-#include "imstkCamera.h"
-#include "imstkCollisionGraph.h"
-#include "imstkCompositeImplicitGeometry.h"
-#include "imstkGeometryUtilities.h"
-#include "imstkImplicitGeometryToImageData.h"
-#include "imstkKeyboardDeviceClient.h"
-#include "imstkKeyboardSceneControl.h"
-#include "imstkDirectionalLight.h"
-#include "imstkLogger.h"
-#include "imstkMouseSceneControl.h"
-#include "imstkNew.h"
-#include "imstkOrientedBox.h"
-#include "imstkPlane.h"
-#include "imstkRbdConstraint.h"
-#include "imstkRenderMaterial.h"
-#include "imstkRigidBodyCH.h"
-#include "imstkRigidBodyModel2.h"
-#include "imstkRigidObject2.h"
-#include "imstkRigidObjectCollision.h"
-#include "imstkScene.h"
-#include "imstkSceneManager.h"
-#include "imstkSimulationManager.h"
-#include "imstkSphere.h"
-#include "imstkSurfaceMesh.h"
-#include "imstkSurfaceMeshFlyingEdges.h"
-#include "imstkSurfaceMeshSubdivide.h"
-#include "imstkVisualModel.h"
-#include "imstkVTKViewer.h"
-#include "imstkCollisionDataDebugObject.h"
-#include "imstkCollisionDetectionAlgorithm.h"
-
-using namespace imstk;
-
-///
-/// \brief Creates plane geometry
-/// \param cloth width
-/// \param cloth height
-/// \param cloth row count
-/// \param cloth column count
-///
-static std::shared_ptr<SurfaceMesh>
-makePlane(const Vec2d size,
-          const Vec2i dim)
-{
-    imstkNew<SurfaceMesh> clothMesh;
-
-    imstkNew<VecDataArray<double, 3>> verticesPtr(dim[0] * dim[1]);
-    VecDataArray<double, 3>&          vertices = *verticesPtr.get();
-    const Vec2d                       dx       = Vec2d(size[0] / static_cast<double>(dim[0] - 1), size[1] / static_cast<double>(dim[1] - 1));
-    const Vec3d                       shift    = -Vec3d(size[0], 0.0, size[1]) * 0.5;
-    for (int i = 0; i < dim[1]; i++)
-    {
-        for (int j = 0; j < dim[0]; j++)
-        {
-            vertices[i * dim[0] + j] = Vec3d(dx[0] * static_cast<double>(i), 0.0, dx[1] * static_cast<double>(j)) + shift;
-        }
-    }
-
-    // Add connectivity data
-    imstkNew<VecDataArray<int, 3>> indicesPtr;
-    VecDataArray<int, 3>&          indices = *indicesPtr.get();
-    for (int i = 0; i < dim[1] - 1; i++)
-    {
-        for (int j = 0; j < dim[0] - 1; j++)
-        {
-            const int index1 = i * dim[0] + j;
-            const int index2 = index1 + dim[0];
-            const int index3 = index1 + 1;
-            const int index4 = index2 + 1;
-
-            // Interleave [/][\]
-            if (i % 2 ^ j % 2)
-            {
-                indices.push_back(Vec3i(index1, index3, index2));
-                indices.push_back(Vec3i(index4, index2, index3));
-            }
-            else
-            {
-                indices.push_back(Vec3i(index2, index1, index4));
-                indices.push_back(Vec3i(index4, index1, index3));
-            }
-        }
-    }
-
-    clothMesh->initialize(verticesPtr, indicesPtr);
-
-    return clothMesh;
-}
-
-///
-/// \brief This examples demonstrates rigid bodies with mesh vs mesh collision
-/// with per triangle/local mesh CD
-/// \todo: This example is a work in progress
-///
-int
-main()
-{
-    // Write log to stdout and file
-    Logger::startLogger();
-
-    imstkNew<Scene> scene("RbdMeshMeshCollision");
-
-    // This model is shared among interacting rigid bodies
-    imstkNew<RigidBodyModel2> rbdModel;
-    rbdModel->getConfig()->m_gravity = Vec3d(0.0, -2500.0, 0.0);
-    rbdModel->getConfig()->m_maxNumIterations = 10;
-
-    // Create the first rbd, plane floor
-    imstkNew<CollidingObject> planeObj("Plane");
-    {
-        std::shared_ptr<SurfaceMesh> planeMesh = makePlane(Vec2d(25.0, 25.0), Vec2i(10, 10));
-
-        // Create the object
-        planeObj->setVisualGeometry(planeMesh);
-        planeObj->setCollidingGeometry(planeMesh);
-        planeObj->getVisualModel(0)->getRenderMaterial()->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
-
-        scene->addSceneObject(planeObj);
-    }
-
-    // Create surface mesh cube (so we can use pointset for point->implicit collision)
-    imstkNew<RigidObject2> cubeObj("Cube");
-    {
-        imstkNew<OrientedBox>        cubeGeom(Vec3d::Zero(), Vec3d(1.5, 3.0, 1.0));
-        std::shared_ptr<SurfaceMesh> cubeSurfMesh = GeometryUtils::toSurfaceMesh(cubeGeom);
-
-        // Create the visual model
-        imstkNew<VisualModel> visualModel;
-        visualModel->setGeometry(cubeSurfMesh);
-        imstkNew<RenderMaterial> mat;
-        mat->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
-        mat->setLineWidth(2.0);
-        mat->setColor(Color::Orange);
-        visualModel->setRenderMaterial(mat);
-
-        // Create the cube rigid object
-        cubeObj->setDynamicalModel(rbdModel);
-        cubeObj->setPhysicsGeometry(cubeSurfMesh);
-        cubeObj->setCollidingGeometry(cubeSurfMesh);
-        cubeObj->addVisualModel(visualModel);
-        cubeObj->getRigidBody()->m_mass    = 100.0;
-        cubeObj->getRigidBody()->m_initPos = Vec3d(0.0, 8.0, 0.0);
-        cubeObj->getRigidBody()->m_initOrientation = Quatd(Rotd(0.4, Vec3d(1.0, 0.0, 0.0)));
-        cubeObj->getRigidBody()->m_intertiaTensor  = Mat3d::Identity();
-
-        scene->addSceneObject(cubeObj);
-    }
-
-    // Collision Interaction
-    auto rbdInteraction = std::make_shared<RigidObjectCollision>(cubeObj, planeObj, "SurfaceMeshToSurfaceMeshCD");
-    rbdInteraction->setFriction(0.0);
-    rbdInteraction->setStiffness(0.05);
-    scene->getCollisionGraph()->addInteraction(rbdInteraction);
-
-    // Camera
-    scene->getActiveCamera()->setPosition(0.0, 40.0, 40.0);
-
-    // Debug Object to display collision data
-    imstkNew<CollisionDataDebugObject> debugObject;
-    debugObject->setInputCD(rbdInteraction->getCollisionDetection()->getCollisionData());
-    scene->addSceneObject(debugObject);
-
-    // Light
-    imstkNew<DirectionalLight> light;
-    light->setIntensity(1.0);
-    scene->addLight("light", light);
-
-    // Run the simulation
-    {
-        // Setup a viewer to render in its own thread
-        imstkNew<VTKViewer> viewer("Viewer");
-        viewer->setActiveScene(scene);
-
-        // Setup a scene manager to advance the scene in its own thread
-        imstkNew<SceneManager> sceneManager("Scene Manager");
-        sceneManager->setActiveScene(scene);
-        sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
-        sceneManager->pause();
-
-        imstkNew<SimulationManager> driver;
-        driver->addModule(viewer);
-        driver->addModule(sceneManager);
-        driver->setDesiredDt(0.01);
-
-        // Add mouse and keyboard controls to the viewer
-        {
-            imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
-            mouseControl->setSceneManager(sceneManager);
-            viewer->addControl(mouseControl);
-
-            imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
-            keyControl->setSceneManager(sceneManager);
-            keyControl->setModuleDriver(driver);
-            viewer->addControl(keyControl);
-        }
-
-        LOG(INFO) << "Cube Controls:";
-        LOG(INFO) << "----------------------------------------------------------------------";
-        LOG(INFO) << " | i - forward movement";
-        LOG(INFO) << " | j - left movement";
-        LOG(INFO) << " | l - right movement";
-        LOG(INFO) << " | k - backwards movement";
-        LOG(INFO) << " | u - rotate left";
-        LOG(INFO) << " | o - rotate right";
-
-        // Not perfectly thread safe movement lambda, ijkl movement instead of wasd because d is already used
-        std::shared_ptr<KeyboardDeviceClient> keyDevice = viewer->getKeyboardDevice();
-        const Vec3d                           dx = scene->getActiveCamera()->getPosition() - scene->getActiveCamera()->getFocalPoint();
-        connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
-        {
-            Vec3d extForce  = Vec3d(0.0, 0.0, 0.0);
-            Vec3d extTorque = Vec3d(0.0, 0.0, 0.0);
-            // If i down, move forward
-            if (keyDevice->getButton('i') == KEY_PRESS)
-            {
-                extForce += Vec3d(0.0, 0.0, -900.0);
-            }
-            if (keyDevice->getButton('k') == KEY_PRESS)
-            {
-                extForce += Vec3d(0.0, 0.0, 900.0);
-            }
-            if (keyDevice->getButton('j') == KEY_PRESS)
-            {
-                extForce += Vec3d(-900.0, 0.0, 0.0);
-            }
-            if (keyDevice->getButton('l') == KEY_PRESS)
-            {
-                extForce += Vec3d(900.0, 0.0, 0.0);
-            }
-            if (keyDevice->getButton('u') == KEY_PRESS)
-            {
-                extTorque += Vec3d(0.0, 1.5, 0.0);
-            }
-            if (keyDevice->getButton('o') == KEY_PRESS)
-            {
-                extTorque += Vec3d(0.0, -1.5, 0.0);
-            }
-            *cubeObj->getRigidBody()->m_force  = extForce;
-            *cubeObj->getRigidBody()->m_torque = extTorque;
-            scene->getActiveCamera()->setFocalPoint(cubeObj->getRigidBody()->getPosition());
-            scene->getActiveCamera()->setPosition(cubeObj->getRigidBody()->getPosition() + dx);
-            debugObject->debugUpdate();
-
-            // Keep cube updating at real time
-            cubeObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
-        });
-
-        driver->start();
-    }
-
-    return 0;
-}
diff --git a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp
deleted file mode 100644
index 18bfd8552a20d2cf01a907fe2df696e724b43772..0000000000000000000000000000000000000000
--- a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.cpp
+++ /dev/null
@@ -1,277 +0,0 @@
-///*=========================================================================
-//
-//   Library: iMSTK
-//
-//   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
-//   & Imaging in Medicine, Rensselaer Polytechnic Institute.
-//
-//   Licensed under the Apache License, Version 2.0 (the "License");
-//   you may not use this file except in compliance with the License.
-//   You may obtain a copy of the License at
-//
-//      http://www.apache.org/licenses/LICENSE-2.0.txt
-//
-//   Unless required by applicable law or agreed to in writing, software
-//   distributed under the License is distributed on an "AS IS" BASIS,
-//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-//   See the License for the specific language governing permissions and
-//   limitations under the License.
-//
-//=========================================================================*/
-//
-//#include "imstkSurfaceMeshToSurfaceMeshCCD.h"
-//#include "imstkCollisionData.h"
-//#include "imstkSurfaceMesh.h"
-//
-//#include <fcl/narrowphase/collision_object.h>
-//#include <fcl/narrowphase/collision_request.h>
-//#include <fcl/narrowphase/collision_result.h>
-//
-//namespace imstk
-//{
-//SurfaceMeshToSurfaceMeshCCD::SurfaceMeshToSurfaceMeshCCD() :
-//    m_modelA(std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>()),
-//    m_modelB(std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>())
-//{
-//    setInputPortReq<SurfaceMesh>(0);
-//    setInputPortReq<SurfaceMesh>(1);
-//
-//    // By default generate contact data for both sides
-//    setGenerateCD(true, true);
-//}
-//
-//void
-//SurfaceMeshToSurfaceMeshCCD::setup(std::shared_ptr<SurfaceMesh> surfMeshA, std::shared_ptr<SurfaceMesh> surfMeshB)
-//{
-//    //if (m_meshA == surfMeshA && m_meshB == surfMeshB)
-//    //{
-//    //    return;
-//    //}
-//
-//    //m_meshA = surfMeshA;
-//    //m_meshB = surfMeshB;
-//
-//    //m_triangleIndicesA = std::vector<std::array<size_t, 3>>(surfMeshA->getNumTriangles());
-//    //m_triangleIndicesB = std::vector<std::array<size_t, 3>>(surfMeshB->getNumTriangles());
-//
-//    //// Copy data A
-//    //const VecDataArray<int, 3>& indicesA = *surfMeshA->getTriangleIndices();
-//    //for (int i = 0; i < surfMeshA->getNumTriangles(); i++)
-//    //{
-//    //    for (int j = 0; j < 3; j++)
-//    //    {
-//    //        m_triangleIndicesA[i][j] = indicesA[i][j];
-//    //    }
-//    //}
-//    //const VecDataArray<double, 3>& positionsA = *surfMeshA->getVertexPositions();
-//    //m_positionsA.resize(positionsA.size());
-//    //for (int i = 0; i < surfMeshA->getNumVertices(); i++)
-//    //{
-//    //    m_positionsA[i] = positionsA[i];
-//    //}
-//
-//    //// Copy data B
-//    //const VecDataArray<int, 3>& indicesB = *surfMeshB->getTriangleIndices();
-//    //for (int i = 0; i < surfMeshB->getNumTriangles(); i++)
-//    //{
-//    //    for (int j = 0; j < 3; j++)
-//    //    {
-//    //        m_triangleIndicesB[i][j] = indicesB[i][j];
-//    //    }
-//    //}
-//    //const VecDataArray<double, 3>& positionsB = *surfMeshB->getVertexPositions();
-//    //m_positionsB.resize(positionsB.size());
-//    //for (int i = 0; i < surfMeshB->getNumVertices(); i++)
-//    //{
-//    //    m_positionsB[i] = positionsB[i];
-//    //}
-//
-//    //m_modelA = std::make_shared<DeformModel>(m_positionsA, m_triangleIndicesA);
-//    //m_modelB = std::make_shared<DeformModel>(m_positionsB, m_triangleIndicesB);
-//
-//    //// Setup Callbacks
-//    //m_modelA->SetEECallBack(SurfaceMeshToSurfaceMeshCCD::EECallback, this);
-//    //m_modelA->SetVFCallBack(SurfaceMeshToSurfaceMeshCCD::VFCallbackA, this);
-//    //m_modelB->SetVFCallBack(SurfaceMeshToSurfaceMeshCCD::VFCallbackB, this);
-//
-//    //// Build BVH
-//    //m_modelA->BuildBVH(false);
-//    //m_modelB->BuildBVH(false);
-//}
-//
-//void
-//SurfaceMeshToSurfaceMeshCCD::computeCollisionDataAB(
-//    std::shared_ptr<Geometry>          geomA,
-//    std::shared_ptr<Geometry>          geomB,
-//    CDElementVector<CollisionElement>& elementsA,
-//    CDElementVector<CollisionElement>& elementsB)
-//{
-//    std::shared_ptr<SurfaceMesh> surfMeshA = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
-//    std::shared_ptr<SurfaceMesh> surfMeshB = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
-//
-//    fcl::Transform3d poseA = fcl::Transform3d::Identity();
-//
-//    VecDataArray<double, 3>& verticesA = *surfMeshA->getVertexPositions();
-//    if (verticesA.size() != m_positionsA.size())
-//    {
-//        m_positionsA.resize(verticesA.size());
-//    }
-//    std::copy_n(verticesA.getPointer(), verticesA.size(), m_positionsA.data());
-//    const VecDataArray<int, 3>& indicesA = *surfMeshA->getTriangleIndices();
-//    if (indicesA.size() != m_fclIndicesA.size())
-//    {
-//        m_fclIndicesA.resize(indicesA.size());
-//        for (int i = 0; i < indicesA.size(); i++)
-//        {
-//            for (int j = 0; j < 3; j++)
-//            {
-//                m_fclIndicesA[i][j] = indicesA[i][j];
-//            }
-//        }
-//    }
-//    fcl::BVHModel<fcl::OBBRSSd> modelA;
-//    modelA.beginModel();
-//    modelA.addSubModel(m_positionsA, m_fclIndicesA);
-//    modelA.endModel();
-//
-//
-//    fcl::Transform3d poseB = fcl::Transform3d::Identity();
-//
-//    VecDataArray<double, 3>& verticesB = *surfMeshB->getVertexPositions();
-//    if (verticesB.size() != m_positionsB.size())
-//    {
-//        m_positionsB.resize(verticesB.size());
-//    }
-//    std::copy_n(verticesB.getPointer(), verticesB.size(), m_positionsB.data());
-//    const VecDataArray<int, 3>& indicesB = *surfMeshB->getTriangleIndices();
-//    if (indicesB.size() != m_fclIndicesB.size())
-//    {
-//        m_fclIndicesB.resize(indicesB.size());
-//        for (int i = 0; i < indicesB.size(); i++)
-//        {
-//            for (int j = 0; j < 3; j++)
-//            {
-//                m_fclIndicesB[i][j] = indicesB[i][j];
-//            }
-//        }
-//    }
-//    fcl::BVHModel<fcl::OBBRSSd> modelB;
-//    modelB.beginModel();
-//    modelB.addSubModel(m_positionsB, m_fclIndicesB);
-//    modelB.endModel();
-//
-//    // set the distance request structure, here we just use the default setting
-//    fcl::CollisionRequestd request;
-//    request.enable_contact = true;
-//    // result will be returned via the collision result structure
-//    fcl::CollisionResultd result;
-//
-//    const size_t numContacts = fcl::collide<double>(&modelA, poseA, &modelB, poseB, request, result);
-//
-//    for (size_t i = 0; i < result.numContacts(); i++)
-//    {
-//        fcl::Contactd contact = result.getContact(i);
-//        if (contact.b1 != -1)
-//        {
-//            // How do I know if b1 refers to a point or triangle?
-//        }
-//    }
-//}
-//
-//VecDataArray<int, 3>&
-//SurfaceMeshToSurfaceMeshCCD::getIndicesA()
-//{
-//    return *m_meshA->getTriangleIndices();
-//}
-//
-//VecDataArray<int, 3>&
-//SurfaceMeshToSurfaceMeshCCD::getIndicesB()
-//{
-//    return *m_meshB->getTriangleIndices();
-//}
-//
-//void
-//SurfaceMeshToSurfaceMeshCCD::EECallback(unsigned int eA_v1, unsigned int eA_v2,
-//                                        unsigned int eB_v1, unsigned int eB_v2,
-//                                        float t, void* userdata)
-//{
-//    auto CD = reinterpret_cast<SurfaceMeshToSurfaceMeshCCD*>(userdata);
-//    if (CD == nullptr)
-//    {
-//        return;
-//    }
-//
-//    CDElementVector<CollisionElement>& elementsA = CD->getElementsA();
-//    CDElementVector<CollisionElement>& elementsB = CD->getElementsB();
-//
-//    EdgeIndexElement elemA;
-//    elemA.p1Index = eA_v1;
-//    elemA.p2Index = eA_v2;
-//
-//    EdgeIndexElement elemB;
-//    elemB.p1Index = eB_v1;
-//    elemB.p2Index = eB_v2;
-//
-//    // Does this need to be safe append?? can this callback occur in parallel?
-//    elementsA.safeAppend(elemA);
-//    elementsB.safeAppend(elemB);
-//
-//    //LOG(INFO) <<"EE: eA("<<eA_v1<<", "<<eA_v2<<"), eB("<<eB_v1<<", "<<eB_v2<<") \t@ t="<<t;
-//}
-//
-//void
-//SurfaceMeshToSurfaceMeshCCD::VFCallbackA(unsigned int fidA, unsigned int vidB,
-//                                         float t, void* userdata)
-//{
-//    auto CD = reinterpret_cast<SurfaceMeshToSurfaceMeshCCD*>(userdata);
-//    if (CD == nullptr)
-//    {
-//        return;
-//    }
-//
-//    CDElementVector<CollisionElement>& elementsA = CD->getElementsA();
-//    CDElementVector<CollisionElement>& elementsB = CD->getElementsB();
-//    VecDataArray<int, 3>&              indicesA  = CD->getIndicesA();
-//
-//    TriangleIndexElement elemA;
-//    elemA.p1Index = indicesA[fidA][0];
-//    elemA.p2Index = indicesA[fidA][1];
-//    elemA.p3Index = indicesA[fidA][2];
-//
-//    PointIndexElement elemB;
-//    elemB.ptIndex = vidB;
-//
-//    elementsA.safeAppend(elemA);
-//    elementsB.safeAppend(elemB);
-//
-//    //LOG(INFO) <<"VF: fA("<<fidA<<"), vB("<<vidB<<") \t\t@ t="<<t;
-//}
-//
-//void
-//SurfaceMeshToSurfaceMeshCCD::VFCallbackB(unsigned int fidB, unsigned int vidA,
-//                                         float t, void* userdata)
-//{
-//    auto CD = reinterpret_cast<SurfaceMeshToSurfaceMeshCCD*>(userdata);
-//    if (CD == nullptr)
-//    {
-//        return;
-//    }
-//
-//    CDElementVector<CollisionElement>& elementsA = CD->getElementsA();
-//    CDElementVector<CollisionElement>& elementsB = CD->getElementsB();
-//    VecDataArray<int, 3>&              indicesB  = CD->getIndicesB();
-//
-//    PointIndexElement elemA;
-//    elemA.ptIndex = vidA;
-//
-//    TriangleIndexElement elemB;
-//    elemB.p1Index = indicesB[fidB][0];
-//    elemB.p2Index = indicesB[fidB][1];
-//    elemB.p3Index = indicesB[fidB][2];
-//
-//    elementsA.safeAppend(elemA);
-//    elementsB.safeAppend(elemB);
-//
-//    //LOG(INFO) <<"VF: vA("<<vidA<<"), fB("<<fidB<<") \t\t@ t="<<t;
-//}
-//}
\ No newline at end of file
diff --git a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.h b/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.h
deleted file mode 100644
index d30a96586e756fd886a42f2218e4f008bcda0a9d..0000000000000000000000000000000000000000
--- a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCCD.h
+++ /dev/null
@@ -1,105 +0,0 @@
-///*=========================================================================
-//
-//   Library: iMSTK
-//
-//   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
-//   & Imaging in Medicine, Rensselaer Polytechnic Institute.
-//
-//   Licensed under the Apache License, Version 2.0 (the "License");
-//   you may not use this file except in compliance with the License.
-//   You may obtain a copy of the License at
-//
-//      http://www.apache.org/licenses/LICENSE-2.0.txt
-//
-//   Unless required by applicable law or agreed to in writing, software
-//   distributed under the License is distributed on an "AS IS" BASIS,
-//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-//   See the License for the specific language governing permissions and
-//   limitations under the License.
-//
-//=========================================================================*/
-//
-//#pragma once
-//
-//#include "imstkCollisionDetectionAlgorithm.h"
-//
-//#include <fcl/fcl.h>
-//#include <fcl/geometry/bvh/BVH_model.h>
-//#include <fcl/math/bv/OBBRSS.h>
-//#include <vector>
-//#include <array>
-//
-//namespace imstk
-//{
-//class SurfaceMesh;
-//
-/////
-///// \class SurfaceMeshToSurfaceMeshCCD
-/////
-///// \brief Continuous collision detection for surface meshes
-/////
-//class SurfaceMeshToSurfaceMeshCCD : public CollisionDetectionAlgorithm
-//{
-//public:
-//    SurfaceMeshToSurfaceMeshCCD();
-//    virtual ~SurfaceMeshToSurfaceMeshCCD() override = default;
-//
-//    ///
-//    /// \brief Returns collision detection type string name
-//    ///
-//    virtual const std::string getTypeName() const override { return "SurfaceMeshToSurfaceMeshCCD"; }
-//
-//public:
-//    ///
-//    /// \brief Compute collision data for AB simulatenously
-//    ///
-//    virtual void computeCollisionDataAB(
-//        std::shared_ptr<Geometry>          geomA,
-//        std::shared_ptr<Geometry>          geomB,
-//        CDElementVector<CollisionElement>& elementsA,
-//        CDElementVector<CollisionElement>& elementsB) override;
-//
-//protected:
-//    void setup(std::shared_ptr<SurfaceMesh> geomA, std::shared_ptr<SurfaceMesh> geomB);
-//
-//protected:
-//    CDElementVector<CollisionElement>& getElementsA() { return *m_elementsA; }
-//    CDElementVector<CollisionElement>& getElementsB() { return *m_elementsB; }
-//
-//    VecDataArray<int, 3>& getIndicesA();
-//    VecDataArray<int, 3>& getIndicesB();
-//
-//    ///
-//    /// \brief Callback defining edge to edge collision (A - B)
-//    ///
-//    static void EECallback(unsigned int eA_v1, unsigned int eA_v2,
-//                           unsigned int eB_v1, unsigned int eB_v2,
-//                           float t, void* userdata);
-//    ///
-//    /// \brief Callback defining vertex to triangle collision (A - B)
-//    ///
-//    static void VFCallbackA(unsigned int fidA, unsigned int vidB,
-//                            float t, void* userdata);
-//    ///
-//    /// \brief Callback defining vertex to triangle collision (B - A)
-//    ///
-//    static void VFCallbackB(unsigned int fidB, unsigned int vidA,
-//                            float t, void* userdata);
-//
-//protected:
-//    std::shared_ptr<SurfaceMesh> m_meshA;   ///> Mesh A
-//    std::shared_ptr<SurfaceMesh> m_meshB;   ///> Mesh B
-//
-//    std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> m_modelA;
-//    std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> m_modelB;
-//
-//    std::vector<Vec3d> m_positionsA;
-//    std::vector<fcl::Triangle> m_fclIndicesA;
-//
-//    std::vector<Vec3d> m_positionsB;
-//    std::vector<fcl::Triangle> m_fclIndicesB;
-//
-//    CDElementVector<CollisionElement>* m_elementsA;
-//    CDElementVector<CollisionElement>* m_elementsB;
-//};
-//}
\ No newline at end of file
diff --git a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCD.cpp
index f86df41f060edfff1d156c511f39411c2946e035..d3b689461ae562b2c5908ecd8e6c56339d134e9d 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToSurfaceMeshCD.cpp
@@ -20,17 +20,10 @@
 =========================================================================*/
 
 #include "imstkSurfaceMeshToSurfaceMeshCD.h"
-#include "imstkCollisionData.h"
 #include "imstkCollisionUtils.h"
 #include "imstkSurfaceMesh.h"
-#include "imstkTimer.h"
-
 #include "imstkGeometryUtilities.h"
 
-#include <vtkCollisionDetectionFilter.h>
-#include <vtkIdTypeArray.h>
-#include <vtkMatrix4x4.h>
-
 struct EdgePair
 {
     EdgePair(uint32_t a1, uint32_t a2, uint32_t b1, uint32_t b2)
@@ -108,168 +101,6 @@ SurfaceMeshToSurfaceMeshCD::SurfaceMeshToSurfaceMeshCD()
     setGenerateCD(true, true);
 }
 
-//void
-//SurfaceMeshToSurfaceMeshCD::computeCollisionDataAB(
-//    std::shared_ptr<Geometry>          geomA,
-//    std::shared_ptr<Geometry>          geomB,
-//    CDElementVector<CollisionElement>& elementsA,
-//    CDElementVector<CollisionElement>& elementsB)
-//{
-//    std::shared_ptr<SurfaceMesh>             surfMeshA    = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
-//    std::shared_ptr<VecDataArray<double, 3>> verticesAPtr = surfMeshA->getVertexPositions();
-//    VecDataArray<double, 3>&                 verticesA    = *verticesAPtr;
-//    std::shared_ptr<VecDataArray<int, 3>>    indicesAPtr  = surfMeshA->getTriangleIndices();
-//    const VecDataArray<int, 3>&              indicesA     = *indicesAPtr;
-//
-//    std::shared_ptr<SurfaceMesh>             surfMeshB    = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
-//    std::shared_ptr<VecDataArray<double, 3>> verticesBPtr = surfMeshB->getVertexPositions();
-//    VecDataArray<double, 3>&                 verticesB    = *verticesBPtr;
-//    std::shared_ptr<VecDataArray<int, 3>>    indicesBPtr  = surfMeshB->getTriangleIndices();
-//    const VecDataArray<int, 3>&              indicesB     = *indicesBPtr;
-//
-//    vtkSmartPointer<vtkPolyData>        polyDataA = GeometryUtils::copyToVtkPolyData(surfMeshA);
-//    vtkSmartPointer<vtkPolyData>        polyDataB = GeometryUtils::copyToVtkPolyData(surfMeshB);
-//    vtkNew<vtkCollisionDetectionFilter> filter;
-//    filter->SetInputData(0, polyDataA);
-//    filter->SetInputData(1, polyDataB);
-//    filter->SetGenerateScalars(false);
-//    vtkNew<vtkMatrix4x4> mat;
-//    mat->Identity();
-//    filter->SetMatrix(0, mat);
-//    filter->SetMatrix(1, mat);
-//    filter->Update();
-//
-//    // Field data on the cells of A and B that give the ids of the other geometries cells intersecting it
-//    vtkIdTypeArray* contactCellsA = filter->GetContactCells(0);
-//    vtkIdTypeArray* contactCellsB = filter->GetContactCells(1);
-//
-//    m_intersectingPairs.resize(0);
-//    for (int i = 0; i < contactCellsA->GetNumberOfValues(); i++)
-//    {
-//        vtkIdType contactCellIdB = contactCellsA->GetValue(i);
-//        vtkIdType contactCellIdA = contactCellsB->GetValue(contactCellIdB);
-//
-//        m_intersectingPairs.push_back(std::pair<int, int>(contactCellIdA, contactCellIdB));
-//    }
-//
-//    // Limit contacts if requested
-//    int numContacts = m_intersectingPairs.size();
-//    if (m_maxNumContacts != -1)
-//    {
-//        numContacts = std::min(m_maxNumContacts, numContacts);
-//    }
-//
-//    std::unordered_set<EdgePair> edges;
-//    for (int i = 0; i < numContacts; i++)
-//    {
-//        const std::pair<int, int>& iPair = m_intersectingPairs[i];
-//        if (iPair.first < 0 || iPair.first >= indicesB.size() ||
-//            iPair.second < 0 || iPair.second >= indicesA.size())
-//        {
-//            printf("wtf %d, %d\n", iPair.first, iPair.second);
-//            continue;
-//        }
-//        const Vec3i&               cellA = indicesA[iPair.second];
-//        const Vec3i&               cellB = indicesB[iPair.first];
-//
-//        // vtContact needs to be checked both ways but eeContact is symmetric
-//        std::pair<Vec2i, Vec2i> eeContact;
-//        std::pair<int, Vec3i>   vtContact;
-//        std::pair<Vec3i, int>   tvContact;
-//        const int               contactType = CollisionUtils::triangleToTriangle(cellA, cellB,
-//            verticesA[cellA[0]], verticesA[cellA[1]], verticesA[cellA[2]],
-//            verticesB[cellB[0]], verticesB[cellB[1]], verticesB[cellB[2]],
-//            eeContact, vtContact, tvContact);
-//
-//        // If you want to visualize the cells in contact
-//        // report triangle vs triangle instead
-//        /* CellIndexElement elemB;
-//        elemB.idCount = 3;
-//        elemB.cellType = IMSTK_TRIANGLE;
-//        elemB.ids[0] = cellB[0];
-//        elemB.ids[1] = cellB[1];
-//        elemB.ids[2] = cellB[2];
-//        CellIndexElement elemA;
-//        elemA.idCount = 3;
-//        elemA.cellType = IMSTK_TRIANGLE;
-//        elemA.ids[0] = cellA[0];
-//        elemA.ids[1] = cellA[1];
-//        elemA.ids[2] = cellA[2];
-//        elementsA.unsafeAppend(elemA);
-//        elementsB.unsafeAppend(elemB);*/
-//
-//        // Type 1, vertex-triangle contact
-//        if (contactType == 1)
-//        {
-//            CellIndexElement elemA;
-//            elemA.idCount  = 1;
-//            elemA.cellType = IMSTK_VERTEX;
-//            elemA.ids[0]   = vtContact.first;
-//
-//            CellIndexElement elemB;
-//            elemB.idCount  = 3;
-//            elemB.cellType = IMSTK_TRIANGLE;
-//            elemB.ids[0]   = vtContact.second[0];
-//            elemB.ids[1]   = vtContact.second[1];
-//            elemB.ids[2]   = vtContact.second[2];
-//
-//            elementsA.unsafeAppend(elemA);
-//            elementsB.unsafeAppend(elemB);
-//        }
-//        // Type 0, edge-edge contact
-//        else if (contactType == 0)
-//        {
-//            // Create an edge pair and hash it to see if we already have this contact from
-//            // another triangle
-//            const EdgePair edgePair = {
-//                static_cast<uint32_t>(eeContact.first[0]),
-//                static_cast<uint32_t>(eeContact.first[1]),
-//                static_cast<uint32_t>(eeContact.second[0]),
-//                static_cast<uint32_t>(eeContact.second[1]) };
-//            if (edges.count(edgePair) == 0)
-//            {
-//                CellIndexElement elemA;
-//                elemA.idCount  = 2;
-//                elemA.cellType = IMSTK_EDGE;
-//                elemA.ids[0]   = eeContact.first[0];
-//                elemA.ids[1]   = eeContact.first[1];
-//
-//                CellIndexElement elemB;
-//                elemB.idCount  = 2;
-//                elemB.cellType = IMSTK_EDGE;
-//                elemB.ids[0]   = eeContact.second[0];
-//                elemB.ids[1]   = eeContact.second[1];
-//
-//                elementsA.unsafeAppend(elemA);
-//                elementsB.unsafeAppend(elemB);
-//                edges.insert(edgePair);
-//            }
-//        }
-//        // Type 3, triangle-vertex contact
-//        else if (contactType == 2)
-//        {
-//            CellIndexElement elemA;
-//            elemA.idCount  = 3;
-//            elemA.cellType = IMSTK_TRIANGLE;
-//            elemA.ids[0]   = tvContact.first[0];
-//            elemA.ids[1]   = tvContact.first[1];
-//            elemA.ids[2]   = tvContact.first[2];
-//
-//            CellIndexElement elemB;
-//            elemB.idCount  = 1;
-//            elemB.cellType = IMSTK_VERTEX;
-//            elemB.ids[0]   = tvContact.second;
-//
-//            elementsA.unsafeAppend(elemA);
-//            elementsB.unsafeAppend(elemB);
-//        }
-//        //else
-//        //{
-//        //    // This case is hit in one edge case
-//        //    LOG(WARNING) << "Contact without intersection!";
-//        //}
-//    }
-//}
 void
 SurfaceMeshToSurfaceMeshCD::computeCollisionDataAB(
     std::shared_ptr<Geometry>          geomA,
diff --git a/Source/CollisionDetection/imstkCDObjectFactory.cpp b/Source/CollisionDetection/imstkCDObjectFactory.cpp
index d3d7bd9197059501d4ed0181f84423cebb7a1a72..a7fc5eeefc2412278a038dabed2ef3a66dd488ab 100644
--- a/Source/CollisionDetection/imstkCDObjectFactory.cpp
+++ b/Source/CollisionDetection/imstkCDObjectFactory.cpp
@@ -21,27 +21,18 @@ limitations under the License.
 
 #include "imstkCDObjectFactory.h"
 #include "imstkBidirectionalPlaneToSphereCD.h"
-#include "imstkCapsule.h"
-#include "imstkCylinder.h"
-#include "imstkImplicitGeometry.h"
 #include "imstkImplicitGeometryToPointSetCCD.h"
-//#include "imstkPointSetToSurfaceMeshCD.h"
 #include "imstkImplicitGeometryToPointSetCD.h"
 #include "imstkMeshToMeshBruteForceCD.h"
-#include "imstkPlane.h"
 #include "imstkPointSetToCapsuleCD.h"
 #include "imstkPointSetToOrientedBoxCD.h"
 #include "imstkPointSetToPlaneCD.h"
 #include "imstkPointSetToSphereCD.h"
-#include "imstkSphere.h"
 #include "imstkSphereToCylinderCD.h"
 #include "imstkSphereToSphereCD.h"
-#include "imstkSurfaceMesh.h"
 #include "imstkSurfaceMeshToCapsuleCD.h"
 #include "imstkSurfaceMeshToSphereCD.h"
-#include "imstkSurfaceMeshToSurfaceMeshCCD.h"
 #include "imstkSurfaceMeshToSurfaceMeshCD.h"
-#include "imstkTetrahedralMesh.h"
 #include "imstkTetraToPointSetCD.h"
 #include "imstkUnidirectionalPlaneToSphereCD.h"
 
@@ -69,11 +60,8 @@ std::unordered_map<std::string, std::function<std::shared_ptr<CollisionDetection
     IMSTK_MAKE_CD_CASE(PointSetToPlaneCD, PointSet, Plane),
     IMSTK_MAKE_CD_CASE(PointSetToSphereCD, PointSet, Sphere),
     IMSTK_MAKE_CD_CASE(PointSetToOrientedBoxCD, PointSet, OrientedBox),
-    //IMSTK_MAKE_CD_CASE(PointSetToSpherePickingCD, Plane, Sphere),
-    //IMSTK_MAKE_CD_CASE(PointSetToSurfaceMeshCD, PointSet, SurfaceMesh),
     IMSTK_MAKE_CD_CASE(SphereToCylinderCD, Sphere, Cylinder),
     IMSTK_MAKE_CD_CASE(SphereToSphereCD, Sphere, Sphere),
-    //IMSTK_MAKE_CD_CASE(SurfaceMeshToSurfaceMeshCCD, SurfaceMesh, SurfaceMesh),
     IMSTK_MAKE_CD_CASE(SurfaceMeshToSurfaceMeshCD, SurfaceMesh, SurfaceMesh),
     IMSTK_MAKE_CD_CASE(SurfaceMeshToCapsuleCD, SurfaceMesh, Capsule),
     IMSTK_MAKE_CD_CASE(SurfaceMeshToSphereCD, SurfaceMesh, Sphere),