Merge branch 'main' into issue/870_unsymmetrical

Author: jslee02

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -50,19 +50,32 @@
 #include "dart/dynamics/PyramidShape.hpp"
 #include "dart/dynamics/Shape.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
+#include "dart/dynamics/ShapeNode.hpp"
 #include "dart/dynamics/SoftMeshShape.hpp"
 #include "dart/dynamics/SphereShape.hpp"
 #include "dart/dynamics/VoxelGridShape.hpp"
 
 #include <assimp/scene.h>
 
+#include <algorithm>
 #include <limits>
+#include <string>
+
+#include <cstdint>
 
 namespace dart {
 namespace collision {
 
 namespace {
 
+std::string collisionObjectKey(const FCLCollisionObject* object)
+{
+  if (!object)
+    return "";
+
+  return object->getKey();
+}
+
 bool collisionCallback(
     fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata);
 
@@ -146,6 +159,11 @@ Contact convertContact(
     fcl::CollisionObject* o2,
     const CollisionOption& option);
 
+bool shouldSwapDeterministically(
+    const FCLCollisionObject* fclObj1, const FCLCollisionObject* fclObj2);
+
+void applyDeterministicSwap(Contact& contact);
+
 /// Collision data stores the collision request and the result given by
 /// collision algorithm.
 struct FCLCollisionCallbackData
@@ -852,6 +870,7 @@ void FCLCollisionDetector::refreshCollisionObject(CollisionObject* const object)
 
   fcl->mFCLCollisionObject = std::unique_ptr<fcl::CollisionObject>(
       new fcl::CollisionObject(claimFCLCollisionGeometry(object->getShape())));
+  fcl->mFCLCollisionObject->setUserData(fcl);
 }
 
 //==============================================================================
@@ -1082,16 +1101,16 @@ bool collisionCallback(
   const auto& option = collData->option;
   const auto& filter = option.collisionFilter;
 
-  // Filtering
-  if (filter) {
-    auto collisionObject1 = static_cast<FCLCollisionObject*>(o1->getUserData());
-    auto collisionObject2 = static_cast<FCLCollisionObject*>(o2->getUserData());
-    DART_ASSERT(collisionObject1);
-    DART_ASSERT(collisionObject2);
+  auto collisionObject1 = static_cast<FCLCollisionObject*>(o1->getUserData());
+  auto collisionObject2 = static_cast<FCLCollisionObject*>(o2->getUserData());
+  DART_ASSERT(collisionObject1);
+  DART_ASSERT(collisionObject2);
+  if (!collisionObject1 || !collisionObject2)
+    return collData->done;
 
-    if (filter->ignoresCollision(collisionObject2, collisionObject1))
-      return collData->done;
-  }
+  // Filtering
+  if (filter && filter->ignoresCollision(collisionObject2, collisionObject1))
+    return collData->done;
 
   // Clear previous results
   fclResult.clear();
@@ -1412,6 +1431,15 @@ void postProcessDART(
       } else {
         numContacts++;
       }
+
+      const auto* fclObj1 = static_cast<FCLCollisionObject*>(o1->getUserData());
+      const auto* fclObj2 = static_cast<FCLCollisionObject*>(o2->getUserData());
+      if (shouldSwapDeterministically(fclObj1, fclObj2)) {
+        if (option.enableContact)
+          std::swap(pair1.point, pair2.point);
+        applyDeterministicSwap(pair1);
+        applyDeterministicSwap(pair2);
+      }
     }
 
     // For binary check, return after adding the first contact point to the
@@ -1624,10 +1652,8 @@ Contact convertContact(
 {
   Contact contact;
 
-  contact.collisionObject1
-      = static_cast<FCLCollisionObject*>(o1->getUserData());
-  contact.collisionObject2
-      = static_cast<FCLCollisionObject*>(o2->getUserData());
+  contact.collisionObject1 = static_cast<CollisionObject*>(o1->getUserData());
+  contact.collisionObject2 = static_cast<CollisionObject*>(o2->getUserData());
 
   if (option.enableContact) {
     contact.point = FCLTypes::convertVector3(fclContact.pos);
@@ -1637,9 +1663,40 @@ Contact convertContact(
     contact.triID2 = fclContact.b2;
   }
 
+  // Enforce deterministic ordering across runs and clones. Tie-break on the
+  // underlying FCL object address if keys are identical.
+  const auto fclObj1
+      = static_cast<FCLCollisionObject*>(contact.collisionObject1);
+  const auto fclObj2
+      = static_cast<FCLCollisionObject*>(contact.collisionObject2);
+  if (shouldSwapDeterministically(fclObj1, fclObj2))
+    applyDeterministicSwap(contact);
+
   return contact;
 }
 
+//==============================================================================
+bool shouldSwapDeterministically(
+    const FCLCollisionObject* fclObj1, const FCLCollisionObject* fclObj2)
+{
+  const auto key1 = collisionObjectKey(fclObj1);
+  const auto key2 = collisionObjectKey(fclObj2);
+
+  const auto addr1 = reinterpret_cast<std::uintptr_t>(fclObj1);
+  const auto addr2 = reinterpret_cast<std::uintptr_t>(fclObj2);
+
+  return (key2 < key1) || (key1 == key2 && addr2 < addr1 && addr2 != 0u);
+}
+
+//==============================================================================
+void applyDeterministicSwap(Contact& contact)
+{
+  std::swap(contact.collisionObject1, contact.collisionObject2);
+  std::swap(contact.triID1, contact.triID2);
+
+  contact.normal = -contact.normal;
+}
+
 } // anonymous namespace
 
 } // namespace collision

dart/collision/fcl/FCLCollisionObject.cpp

@@ -34,7 +34,10 @@
 
 #include "dart/collision/fcl/FCLTypes.hpp"
 #include "dart/common/Macros.hpp"
+#include "dart/dynamics/BodyNode.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
+#include "dart/dynamics/ShapeNode.hpp"
+#include "dart/dynamics/Skeleton.hpp"
 #include "dart/dynamics/SoftMeshShape.hpp"
 
 namespace dart {
@@ -63,6 +66,23 @@ FCLCollisionObject::FCLCollisionObject(
     mFCLCollisionObject(new dart::collision::fcl::CollisionObject(fclCollGeom))
 {
   mFCLCollisionObject->setUserData(this);
+
+  if (shapeFrame) {
+    const auto* shapeNode
+        = dynamic_cast<const dynamics::ShapeNode*>(shapeFrame);
+    if (shapeNode) {
+      const auto bodyNode = shapeNode->getBodyNodePtr();
+      const auto skeleton = bodyNode ? bodyNode->getSkeleton() : nullptr;
+
+      if (skeleton)
+        mKey += skeleton->getName() + "::";
+      if (bodyNode)
+        mKey += bodyNode->getName() + "::";
+      mKey += shapeNode->getName();
+    } else {
+      mKey = shapeFrame->getName();
+    }
+  }
 }
 
 //==============================================================================

dart/collision/fcl/FCLCollisionObject.hpp

@@ -36,6 +36,8 @@
 #include <dart/collision/CollisionObject.hpp>
 #include <dart/collision/fcl/BackwardCompatibility.hpp>
 
+#include <string>
+
 namespace dart {
 namespace collision {
 
@@ -50,6 +52,12 @@ class FCLCollisionObject : public CollisionObject
   /// Return FCL collision object
   const dart::collision::fcl::CollisionObject* getFCLCollisionObject() const;
 
+  /// Deterministic key based on the attached ShapeFrame hierarchy.
+  const std::string& getKey() const
+  {
+    return mKey;
+  }
+
 protected:
   /// Constructor
   FCLCollisionObject(
@@ -64,6 +72,9 @@ class FCLCollisionObject : public CollisionObject
 protected:
   /// FCL collision object
   std::unique_ptr<dart::collision::fcl::CollisionObject> mFCLCollisionObject;
+
+  /// Stable identifier for deterministic ordering.
+  std::string mKey;
 };
 
 } // namespace collision

dart/gui/ImGuiHandler.cpp

@@ -48,6 +48,8 @@
 
 #include <algorithm>
 
+#include <cmath>
+
 namespace dart {
 namespace gui {
 
@@ -201,6 +203,29 @@ ConvertedKey convertFromOSGKey(int key)
   }
 }
 
+//==============================================================================
+void applyImGuiScale(float scale)
+{
+  if (!std::isfinite(scale) || scale <= 0.f)
+    return;
+
+  static ImGuiStyle baseStyle
+      = ImGui::GetStyle(); // capture styled defaults (dark palette)
+  auto& style = ImGui::GetStyle();
+  ImGuiIO& io = ImGui::GetIO();
+
+  // Reset to the captured base style before applying the requested scale so
+  // repeated calls are idempotent and preserve the configured palette.
+  style = baseStyle;
+  io.FontGlobalScale = 1.f;
+
+  if (std::abs(scale - 1.f) < 1e-6f)
+    return;
+
+  style.ScaleAllSizes(scale);
+  io.FontGlobalScale = scale;
+}
+
 //==============================================================================
 struct ImGuiNewFrameCallback : public ::osg::Camera::DrawCallback
 {
@@ -237,7 +262,10 @@ struct ImGuiDrawCallback : public ::osg::Camera::DrawCallback
 
 //==============================================================================
 ImGuiHandler::ImGuiHandler()
-  : mTime{0.0}, mMousePressed{false, false, false}, mMouseWheel{0.0f}
+  : mTime{0.0},
+    mMousePressed{false, false, false},
+    mMouseWheel{0.0f},
+    mFramebufferScale{1.0f, 1.0f}
 {
   ImGui::CreateContext();
 
@@ -344,6 +372,18 @@ bool ImGuiHandler::handle(
   const bool wantCaptureMouse = io.WantCaptureMouse;
   const bool wantCaptureKeyboard = io.WantCaptureKeyboard;
 
+  const float invScaleX
+      = mFramebufferScale[0] > 0.0f ? 1.0f / mFramebufferScale[0] : 1.0f;
+  const float invScaleY
+      = mFramebufferScale[1] > 0.0f ? 1.0f / mFramebufferScale[1] : 1.0f;
+
+  const auto getScaledMousePosition
+      = [&](const osgGA::GUIEventAdapter& adapter) {
+          const float scaledX = static_cast<float>(adapter.getX()) * invScaleX;
+          const float scaledY = static_cast<float>(adapter.getY()) * invScaleY;
+          return ImVec2(scaledX, io.DisplaySize.y - scaledY);
+        };
+
   switch (eventAdapter.getEventType()) {
     case osgGA::GUIEventAdapter::KEYDOWN: {
       const int key = eventAdapter.getUnmodifiedKey();
@@ -417,8 +457,7 @@ bool ImGuiHandler::handle(
       return wantCaptureKeyboard;
     }
     case osgGA::GUIEventAdapter::PUSH: {
-      io.MousePos
-          = ImVec2(eventAdapter.getX(), io.DisplaySize.y - eventAdapter.getY());
+      io.MousePos = getScaledMousePosition(eventAdapter);
 
       if (eventAdapter.getButtonMask()
           == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON) {
@@ -440,8 +479,7 @@ bool ImGuiHandler::handle(
     }
     case osgGA::GUIEventAdapter::DRAG:
     case osgGA::GUIEventAdapter::MOVE: {
-      io.MousePos
-          = ImVec2(eventAdapter.getX(), io.DisplaySize.y - eventAdapter.getY());
+      io.MousePos = getScaledMousePosition(eventAdapter);
 
       return wantCaptureMouse;
     }
@@ -489,9 +527,39 @@ void ImGuiHandler::newFrame(::osg::RenderInfo& renderInfo)
   ImGui_ImplOpenGL2_NewFrame();
 
   auto& io = ImGui::GetIO();
-  auto* viewport = renderInfo.getCurrentCamera()->getViewport();
+  auto* camera = renderInfo.getCurrentCamera();
+  auto* viewport = camera ? camera->getViewport() : nullptr;
+
+  ImVec2 displaySize(1.0f, 1.0f);
+  ImVec2 framebufferScale(1.0f, 1.0f);
+
+  if (viewport) {
+    // Use framebuffer-to-window ratio to honor OS display scaling.
+    displaySize = ImVec2(viewport->width(), viewport->height());
+
+    auto* graphicsContext = camera->getGraphicsContext();
+    const auto* traits
+        = graphicsContext ? graphicsContext->getTraits() : nullptr;
+    if (traits && traits->width > 0 && traits->height > 0) {
+      framebufferScale.x = static_cast<float>(viewport->width())
+                           / static_cast<float>(traits->width);
+      framebufferScale.y = static_cast<float>(viewport->height())
+                           / static_cast<float>(traits->height);
+
+      framebufferScale.x
+          = framebufferScale.x > 0.0f ? framebufferScale.x : 1.0f;
+      framebufferScale.y
+          = framebufferScale.y > 0.0f ? framebufferScale.y : 1.0f;
+
+      displaySize = ImVec2(
+          static_cast<float>(traits->width),
+          static_cast<float>(traits->height));
+    }
+  }
 
-  io.DisplaySize = ImVec2(viewport->width(), viewport->height());
+  io.DisplaySize = displaySize;
+  io.DisplayFramebufferScale = framebufferScale;
+  mFramebufferScale = {framebufferScale.x, framebufferScale.y};
 
   const auto currentTime
       = renderInfo.getView()->getFrameStamp()->getSimulationTime();

dart/gui/ImGuiHandler.hpp

@@ -55,6 +55,8 @@ namespace gui {
 
 class ImGuiWidget;
 
+DART_GUI_API void applyImGuiScale(float scale);
+
 class DART_GUI_API ImGuiHandler : public osgGA::GUIEventHandler
 {
 public:
@@ -104,6 +106,8 @@ class DART_GUI_API ImGuiHandler : public osgGA::GUIEventHandler
 
   float mMouseWheel;
 
+  std::array<float, 2> mFramebufferScale;
+
   std::vector<std::shared_ptr<ImGuiWidget>> mWidgets;
 };
 

dart/gui/ImGuiViewer.cpp

@@ -68,6 +68,12 @@ const ImGuiHandler* ImGuiViewer::getImGuiHandler() const
   return mImGuiHandler.get();
 }
 
+//==============================================================================
+void ImGuiViewer::setImGuiScale(float scale)
+{
+  applyImGuiScale(scale);
+}
+
 //==============================================================================
 void ImGuiViewer::showAbout()
 {

dart/gui/ImGuiViewer.hpp

@@ -62,6 +62,9 @@ class DART_GUI_API ImGuiViewer : public Viewer
   /// Get cosnt ImGui handler.
   const ImGuiHandler* getImGuiHandler() const;
 
+  /// Set the ImGui global scale factor (fonts + widget sizes).
+  void setImGuiScale(float scale);
+
   /// Show About widget.
   void showAbout();