restored wrong code cleanup

VirtualRobot/Visualization/TriMeshModel.cpp

@@ -276,11 +276,26 @@ namespace VirtualRobot
         return static_cast<unsigned int>(counter);
     }
 
-    unsigned int TriMeshModel::addMissingColors(const VisualizationFactory::Color& /*color*/)
+    unsigned int TriMeshModel::addMissingColors(const VisualizationFactory::Color& color)
     {
         mergeVertices();
         int counter = 0;
 
+        auto getAndAddColorId = [&](unsigned int vertexId)
+        {
+            for(auto& face : faces)
+            {
+                if(face.id1 == vertexId && face.idColor1 != UINT_MAX)
+                    return face.idColor1;
+                if(face.id2 == vertexId && face.idColor2 != UINT_MAX)
+                    return face.idColor2;
+                if(face.id3 == vertexId && face.idColor3 != UINT_MAX)
+                    return face.idColor3;
+            }
+            return addColor(color);
+        };
+
+
         for (auto& face : faces)
         {
             if (face.idColor1 != UINT_MAX && face.idColor2 != UINT_MAX && face.idColor3 != UINT_MAX)
@@ -288,18 +303,23 @@ namespace VirtualRobot
                 continue;
             }
 
-            if (face.idColor1 == UINT_MAX)
+            auto colorId = UINT_MAX;
+            if(face.idColor1 == UINT_MAX)
             {
+                colorId = face.idColor1 = getAndAddColorId(face.id1);
                 counter++;
             }
-            if (face.idColor2 == UINT_MAX)
+            if(face.idColor2 == UINT_MAX)
             {
+                colorId = face.idColor2 = getAndAddColorId(face.id2);
                 counter++;
             }
-            if (face.idColor3 == UINT_MAX)
+            if(face.idColor3 == UINT_MAX)
             {
+                colorId = face.idColor3 = getAndAddColorId(face.id3);
                 counter++;
             }
+
         }
         return static_cast<unsigned int>(counter);
     }