Efficiently handle 3-bytes pixel formats (#743) (#750)

* Efficiently handle 3-bytes pixel formats Signed-off-by: ahcorde <ahcorde@gmail.com> * Added feedback Signed-off-by: ahcorde <ahcorde@gmail.com> * Fixed windows warnings Signed-off-by: ahcorde <ahcorde@gmail.com>
ros2 · Sep 13, 2021 · a1573af · a1573af
1 parent ba05260
commit a1573af
Showing 1 changed file with 13 additions and 10 deletions.
diff --git a/rviz_common/src/rviz_common/interaction/selection_manager.cpp b/rviz_common/src/rviz_common/interaction/selection_manager.cpp
@@ -261,20 +261,23 @@ void SelectionManager::setHighlightRect(Ogre::Viewport * viewport, int x1, int y
 
 void SelectionManager::unpackColors(const Ogre::PixelBox & box)
 {
-  auto w = box.getWidth();
-  auto h = box.getHeight();
+  uint32_t w = box.getWidth();
+  uint32_t h = box.getHeight();
 
   pixel_buffer_.clear();
   pixel_buffer_.reserve(w * h);
 
-  for (uint32_t y = 0; y < h; ++y) {
-    for (uint32_t x = 0; x < w; ++x) {
-      uint32_t pos = (x + y * w) * 4;
-
-      uint32_t pix_val = *reinterpret_cast<uint32_t *>(static_cast<uint8_t *>(box.data) + pos);
-      uint32_t handle = colorToHandle(box.format, pix_val);
-
-      pixel_buffer_.push_back(handle);
+  size_t size = Ogre::PixelUtil::getMemorySize(1, 1, 1, box.format);
+
+  for (uint32_t y = 0; y < h; y++) {
+    for (uint32_t x = 0; x < w; x++) {
+      uint32_t pos = static_cast<uint32_t>((x + y * w) * size);
+      uint32_t pix_val = 0;
+      memcpy(
+        reinterpret_cast<uint8_t *>(&pix_val),
+        reinterpret_cast<uint8_t *>(box.data + pos),
+        size);
+      pixel_buffer_.push_back(colorToHandle(box.format, pix_val));
     }
   }
 }