From b7f5dd1cee755fe0e7a43c8c83794f22452501c1 Mon Sep 17 00:00:00 2001
From: "Jeongseok (JS) Lee" <jslee02@users.noreply.github.com>
Date: Tue, 19 Dec 2023 06:31:38 -0800
Subject: [PATCH] Attempt to fix #1769 (#1778)

---
 dart/dynamics/BodyNode.cpp                 |  5 ++++-
 dart/dynamics/Skeleton.cpp                 |  7 +++++++
 dart/gui/osg/Viewer.cpp                    | 13 +++++++++++++
 dart/gui/osg/Viewer.hpp                    |  4 +++-
 dart/gui/osg/detail/CameraModeCallback.cpp |  3 +--
 5 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp
index 0fb72950c0a91..b905f37873661 100644
--- a/dart/dynamics/BodyNode.cpp
+++ b/dart/dynamics/BodyNode.cpp
@@ -176,7 +176,10 @@ ConstSkeletonPtr SkeletonRefCountingBase::getSkeleton() const
 /// SET_FLAGS : A version of SKEL_SET_FLAGS that assumes a SkeletonPtr named
 /// 'skel' has already been locked
 #define SET_FLAGS(X)                                                           \
-  skel->mTreeCache[mTreeIndex].mDirty.X = true;                                \
+  if (mTreeIndex != INVALID_INDEX)                                             \
+  {                                                                            \
+    skel->mTreeCache[mTreeIndex].mDirty.X = true;                              \
+  }                                                                            \
   skel->mSkelCache.mDirty.X = true;
 
 /// CHECK_FLAG : Check if the dirty flag X for the tree of this BodyNode is
diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
index 8d2c1bc037fce..aafea5798b91d 100644
--- a/dart/dynamics/Skeleton.cpp
+++ b/dart/dynamics/Skeleton.cpp
@@ -2357,6 +2357,13 @@ void Skeleton::registerNode(Node* _newNode)
 //==============================================================================
 void Skeleton::destructOldTree(std::size_t tree)
 {
+  // Invalidate the tree indices of every BodyNode that is being removed
+  DataCache& treeToDestroy = mTreeCache[tree];
+  for (auto& bodyNode : treeToDestroy.mBodyNodes)
+  {
+    bodyNode->mTreeIndex = INVALID_INDEX;
+  }
+
   mTreeCache.erase(mTreeCache.begin() + tree);
   mTreeNodeMaps.erase(mTreeNodeMaps.begin() + tree);
 
diff --git a/dart/gui/osg/Viewer.cpp b/dart/gui/osg/Viewer.cpp
index ee07a6ac3b6d9..5e6cefa445cb5 100644
--- a/dart/gui/osg/Viewer.cpp
+++ b/dart/gui/osg/Viewer.cpp
@@ -52,6 +52,19 @@ namespace dart {
 namespace gui {
 namespace osg {
 
+std::string toString(CameraMode mode)
+{
+  switch (mode)
+  {
+    case CameraMode::RGBA:
+      return "RGBA";
+    case CameraMode::DEPTH:
+      return "DEPTH";
+    default:
+      return "UNKNOWN";
+  }
+}
+
 class SaveScreen : public ::osg::Camera::DrawCallback
 {
 public:
diff --git a/dart/gui/osg/Viewer.hpp b/dart/gui/osg/Viewer.hpp
index fbf4390f12300..7b908e544a3da 100644
--- a/dart/gui/osg/Viewer.hpp
+++ b/dart/gui/osg/Viewer.hpp
@@ -83,10 +83,12 @@ enum class CameraMode
 
   /// To render the depth buffer
   ///
-  /// \warning The DEPTH mode currently not compatible with the ImGui wigets.
+  /// \warning The DEPTH mode currently not compatible with the ImGui widgets.
   DEPTH,
 };
 
+[[nodiscard]] std::string toString(CameraMode mode);
+
 DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
 class ViewerAttachment : public virtual ::osg::Group
 {
diff --git a/dart/gui/osg/detail/CameraModeCallback.cpp b/dart/gui/osg/detail/CameraModeCallback.cpp
index 7030c894dd6cb..6905decc8044a 100644
--- a/dart/gui/osg/detail/CameraModeCallback.cpp
+++ b/dart/gui/osg/detail/CameraModeCallback.cpp
@@ -131,8 +131,7 @@ void CameraModeCallback::setCameraMode(CameraMode mode)
   if (mode != CameraMode::RGBA && mode != CameraMode::DEPTH)
   {
     DART_WARN(
-        "Unsupported camera mode '{}'. Use RGBA or DEPTH.",
-        static_cast<int>(mode));
+        "Unsupported camera mode '{}'. Use RGBA or DEPTH.", toString(mode));
     return;
   }