diff --git a/CMake/Utilities/imstkAddExecutable.cmake b/CMake/Utilities/imstkAddExecutable.cmake
index 3db3956511607dc97337f89c77540158a18080c3..d9d32251e5caa9449c1f2cbe850cd504646bc86d 100644
--- a/CMake/Utilities/imstkAddExecutable.cmake
+++ b/CMake/Utilities/imstkAddExecutable.cmake
@@ -9,8 +9,25 @@ macro(imstk_add_executable target)
   if (VTK_VERSION VERSION_GREATER_EQUAL  "8.90")
     vtk_module_autoinit(TARGETS ${target} MODULES ${VTK_LIBRARIES})
   endif()
+
+  if (iMSTK_COLOR_OUTPUT)
+    target_compile_options(${target} PRIVATE
+      $<$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>,$<CXX_COMPILER_ID:GNU>>:
+          -Wall -Wno-unused-function -fdiagnostics-color=always>
+      $<$<CXX_COMPILER_ID:MSVC>:
+          -W4 -MP>)
+  else()
+    target_compile_options(${target} PRIVATE
+    $<$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>,$<CXX_COMPILER_ID:GNU>>:
+        -Wall -Wno-unused-function>
+    $<$<CXX_COMPILER_ID:MSVC>:
+        -W4 -MP>)
+  endif()
+
   set_target_properties(${target} PROPERTIES
     DEBUG_POSTFIX "${CMAKE_DEBUG_POSTFIX}")
+
+  # Copy to install directory
   add_custom_command(TARGET ${target} POST_BUILD
                    COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:${target}> ${CMAKE_INSTALL_PREFIX}/bin)
                    
diff --git a/CMake/Utilities/imstkAddLibrary.cmake b/CMake/Utilities/imstkAddLibrary.cmake
index 41901e34ae2560f7c77b68dad47003a13e85c274..20c3ecce5bad5276648aaf26c2cd2056cb33f9db 100644
--- a/CMake/Utilities/imstkAddLibrary.cmake
+++ b/CMake/Utilities/imstkAddLibrary.cmake
@@ -91,11 +91,19 @@ function(imstk_add_library target)
   #-----------------------------------------------------------------------------
   # Set compile flags for the target
   #-----------------------------------------------------------------------------  
-  target_compile_options(${target} PRIVATE
-                           $<$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>,$<CXX_COMPILER_ID:GNU>>:
-                                -Wall -Wno-unused-function>
-                           $<$<CXX_COMPILER_ID:MSVC>:
-                                -W4 -MP>)
+  if (iMSTK_COLOR_OUTPUT)
+    target_compile_options(${target} PRIVATE
+      $<$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>,$<CXX_COMPILER_ID:GNU>>:
+          -Wall -Wno-unused-function -fdiagnostics-color=always>
+      $<$<CXX_COMPILER_ID:MSVC>:
+          -W4 -MP>)
+  else()
+    target_compile_options(${target} PRIVATE
+    $<$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>,$<CXX_COMPILER_ID:GNU>>:
+        -Wall -Wno-unused-function>
+    $<$<CXX_COMPILER_ID:MSVC>:
+        -W4 -MP>)
+  endif()
 
   #-----------------------------------------------------------------------------
   # Install headers
diff --git a/Examples/Vessel/vesselExample.cpp b/Examples/Vessel/vesselExample.cpp
index a7932ec03cd974b1f826af6b394f4dbeb76906ba..c6609fc856fdb9090dee6ea18e689a357d6ce056 100644
--- a/Examples/Vessel/vesselExample.cpp
+++ b/Examples/Vessel/vesselExample.cpp
@@ -227,8 +227,8 @@ main()
         scene->addSceneObject(legsObj);
 
         // Position the camera
-        const Vec6d& bounds = std::dynamic_pointer_cast<SignedDistanceField>(legsObj->getCollidingGeometry())->getBounds();
-        const Vec3d  center = (Vec3d(bounds[0], bounds[2], bounds[4]) + Vec3d(bounds[1], bounds[3], bounds[5])) * 0.5;
+        //const Vec6d& bounds = std::dynamic_pointer_cast<SignedDistanceField>(legsObj->getCollidingGeometry())->getBounds();
+        //const Vec3d  center = (Vec3d(bounds[0], bounds[2], bounds[4]) + Vec3d(bounds[1], bounds[3], bounds[5])) * 0.5;
         scene->getActiveCamera()->setPosition(3.25, 1.6, 3.38);
         scene->getActiveCamera()->setFocalPoint(-2.05, 1.89, -1.32);
         scene->getActiveCamera()->setViewUp(-0.66, 0.01, 0.75);
diff --git a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp
index 942e84243fad6a83b09a86372fca31da4e9d7f24..481fdc523aecdf51273fbb8fe3afc22ed7afad4a 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkImplicitGeometryToPointSetCCD.cpp
@@ -51,7 +51,7 @@ findFirstRoot(std::shared_ptr<ImplicitGeometry> implicitGeomA, const Vec3d& star
     Vec3d  currPos  = start;
     Vec3d  prevPos  = start;
     double currDist = implicitGeomA->getFunctionValue(start);
-    double prevDist = currDist;
+    //double prevDist = currDist;
 
     // Root find (could be multiple roots, we want the first, so start march from front)
     // Gradient could be used for SDFs to converge faster but not for levelsets
@@ -64,7 +64,7 @@ findFirstRoot(std::shared_ptr<ImplicitGeometry> implicitGeomA, const Vec3d& star
         prevPos = currPos;
         currPos = start + dir * x;
 
-        prevDist = currDist;
+        //prevDist = currDist;
         currDist = implicitGeomA->getFunctionValue(currPos);
 
         if (currDist <= 0.0)
@@ -105,7 +105,7 @@ ImplicitGeometryToPointSetCCD::computeCollisionData()
         const Vec3d  prevPt = pt - displacement;
 
         Vec3d  prevPos      = prevPt;
-        double prevDist     = m_implicitGeomA->getFunctionValue(prevPt);
+        double prevDist     = m_implicitGeomA->getFunctionValue(prevPos);
         bool   prevIsInside = std::signbit(prevDist);
 
         Vec3d  currPos      = pt;
@@ -127,17 +127,7 @@ ImplicitGeometryToPointSetCCD::computeCollisionData()
                 {
                     PositionDirectionCollisionDataElement elem;
                     elem.dirAtoB = -centralGrad(contactPt).normalized(); // -centralGrad gives Outward facing contact normal
-                    //elem.dirAtoB = (contactPt - start).normalized();
-                    /* {
-                         Vec3d gradPos = forwardGrad(contactPt);
-                         Vec3d gradNeg = backwardGrad(contactPt);
-                         elem.dirAtoB[0] = std::min(gradNeg[0], 0.0) + std::max(gradPos[0], 0.0);
-                         elem.dirAtoB[1] = std::min(gradNeg[1], 0.0) + std::max(gradPos[1], 0.0);
-                         elem.dirAtoB[2] = std::min(gradNeg[2], 0.0) + std::max(gradPos[2], 0.0);
-                         elem.dirAtoB.normalize();
-                     }*/
                     elem.nodeIdx = static_cast<uint32_t>(i);
-                    //elem.penetrationDepth = (contactPt - pt).norm();
                     elem.penetrationDepth = std::max(0.0, (contactPt - end).dot(elem.dirAtoB));
                     if (elem.penetrationDepth <= limit)
                     {
@@ -159,17 +149,7 @@ ImplicitGeometryToPointSetCCD::computeCollisionData()
             {
                 PositionDirectionCollisionDataElement elem;
                 elem.dirAtoB = -centralGrad(contactPt).normalized(); // -centralGrad gives Outward facing contact normal
-                //elem.dirAtoB = (contactPt - start).normalized();
-                /* {
-                     Vec3d gradPos = forwardGrad(contactPt);
-                     Vec3d gradNeg = backwardGrad(contactPt);
-                     elem.dirAtoB[0] = std::max(gradNeg[0], 0.0) + std::min(gradPos[0], 0.0);
-                     elem.dirAtoB[1] = std::max(gradNeg[1], 0.0) + std::min(gradPos[1], 0.0);
-                     elem.dirAtoB[2] = std::max(gradNeg[2], 0.0) + std::min(gradPos[2], 0.0);
-                     elem.dirAtoB.normalize();
-                 }*/
                 elem.nodeIdx = static_cast<uint32_t>(i);
-                //elem.penetrationDepth = (contactPt - pt).norm();
                 elem.penetrationDepth = std::max(0.0, (contactPt - end).dot(elem.dirAtoB));
                 if (elem.penetrationDepth <= limit)
                 {
@@ -183,7 +163,6 @@ ImplicitGeometryToPointSetCCD::computeCollisionData()
             else
             {
                 prevOuterElementCounter[i] = 0;
-                //printf("Somethings wrong2\n");
             }
         }
         else
diff --git a/Source/CollisionHandling/imstkLevelSetCH.cpp b/Source/CollisionHandling/imstkLevelSetCH.cpp
index 17a276bdb01260ccc385fdb415ac84e2d7cc79f2..fa91068268676ba4e434988a6e3c0a125a215545 100644
--- a/Source/CollisionHandling/imstkLevelSetCH.cpp
+++ b/Source/CollisionHandling/imstkLevelSetCH.cpp
@@ -103,7 +103,7 @@ LevelSetCH::processCollisionData()
     {
         // Apply impulses at points of contacts
         PositionDirectionCollisionData& pdColData = m_colData->PDColData;
-        for (int i = 0; i < pdColData.getSize(); i++)
+        for (size_t i = 0; i < pdColData.getSize(); i++)
         {
             // If the point is in the mask, let it apply impulses
             if (m_ptIdMask.count(pdColData[i].nodeIdx) != 0)
diff --git a/Source/Common/imstkVecDataArray.h b/Source/Common/imstkVecDataArray.h
index d64c60529ee877ca1274d6f835107be0f9a08c20..6685be6d1adb099c40f7f649b108821e52746e12 100644
--- a/Source/Common/imstkVecDataArray.h
+++ b/Source/Common/imstkVecDataArray.h
@@ -297,7 +297,7 @@ public:
 
         if (vecPos != m_vecSize - 1)
         {
-            std::copy_n(m_data + pos + N, AbstractDataArray::m_size - pos - N, DataArray<T>::m_data + pos);
+            std::copy_n(DataArray<T>::m_data + pos + N, AbstractDataArray::m_size - pos - N, DataArray<T>::m_data + pos);
         }
 
         m_vecSize--;
diff --git a/Source/Controllers/imstkRigidObjectController.cpp b/Source/Controllers/imstkRigidObjectController.cpp
index e038fa39eb8481b25d2b0e799e2537e946815ec0..cea86ee63ad0225f8c15ae0591cda11357470f9f 100644
--- a/Source/Controllers/imstkRigidObjectController.cpp
+++ b/Source/Controllers/imstkRigidObjectController.cpp
@@ -135,7 +135,7 @@ RigidObjectController::applyForces()
             {
                 m_forces.push_back(force);
                 m_forceSum += force;
-                if (m_forces.size() > m_smoothingKernelSize)
+                if (static_cast<int>(m_forces.size()) > m_smoothingKernelSize)
                 {
                     m_forceSum -= m_forces.front();
                     m_forces.pop_front();
diff --git a/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp b/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp
index fbb7b9cd28d479c2c3e6c0e8776c375aa5630ecc..c3eb5af92d85c0c512e4c90edf01841a066e52b7 100644
--- a/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkSPHModel.cpp
@@ -715,7 +715,7 @@ SPHModel::moveParticles(const Real timestep)
     VecDataArray<double, 3>& halfStepVelocities = *getCurrentState()->getHalfStepVelocities();
     VecDataArray<double, 3>& fullStepVelocities = *getCurrentState()->getFullStepVelocities();
 
-    for (int p = 0; p < getCurrentState()->getNumParticles(); p++)
+    for (int p = 0; p < static_cast<int>(getCurrentState()->getNumParticles()); p++)
     {
         if (m_sphBoundaryConditions
             && (m_sphBoundaryConditions->getParticleTypes()[p] == SPHBoundaryConditions::ParticleType::Buffer
diff --git a/Source/Filtering/imstkFastMarch.cpp b/Source/Filtering/imstkFastMarch.cpp
index 03cbcf2a27a3402ac056d8dad0e15a20a51991b4..5be3a1001a2f4b574f0d3a2273cd52e79e8ffa5f 100644
--- a/Source/Filtering/imstkFastMarch.cpp
+++ b/Source/Filtering/imstkFastMarch.cpp
@@ -51,9 +51,9 @@ FastMarch::solve()
     m_queue = std::priority_queue<Node, std::vector<Node>, NodeComparator>();
 
     // Add the initial seeds to the queue
-    for (int i = 0; i < m_seedVoxels.size(); i++)
+    for (size_t i = 0; i < m_seedVoxels.size(); i++)
     {
-        Vec3i coord = m_seedVoxels[i];
+        const Vec3i& coord = m_seedVoxels[i];
         if (coord[0] < 0 || coord[0] >= m_dim[0]
             || coord[1] < 0 || coord[1] >= m_dim[1]
             || coord[2] < 0 || coord[2] >= m_dim[2])
diff --git a/Source/SimulationManager/imstkSceneManager.cpp b/Source/SimulationManager/imstkSceneManager.cpp
index 41eb055b8edc63107663ac53498e229fdb17ca73..edc26c50d0b6c9ee320befc2aa93455559f55165 100644
--- a/Source/SimulationManager/imstkSceneManager.cpp
+++ b/Source/SimulationManager/imstkSceneManager.cpp
@@ -26,8 +26,8 @@
 
 namespace imstk
 {
-SceneManager::SceneManager(std::string name) : m_mode(Mode::Simulation),
-    m_activeScene(nullptr), m_prevCamName("default")
+SceneManager::SceneManager(std::string name) : m_activeScene(nullptr),
+    m_mode(Mode::Simulation), m_prevCamName("default")
 {
     // Set the preferred execution mode
     m_executionType = ExecutionType::PARALLEL;