[sim] Add Mechanism2D to Simulation GUI (#2607)

wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/Mechanism2D.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+Mechanism2D::Mechanism2D() : m_device{"Mechanism2D"} {}
+
+void Mechanism2D::SetLigamentAngle(const wpi::Twine& ligamentPath,
+                                   float angle) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/angle").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, angle);
+    }
+    createdItems[fullPath].Set(angle);
+  }
+}
+
+void Mechanism2D::SetLigamentLength(const wpi::Twine& ligamentPath,
+                                    float length) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/length").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, length);
+    }
+    createdItems[fullPath].Set(length);
+  }
+}