From 526dadbcda6c716796f2704f214c40797e4749db Mon Sep 17 00:00:00 2001
From: Kei Usui <kei.usui@mujin.co.jp>
Date: Fri, 1 Mar 2024 16:49:09 -0500
Subject: [PATCH] implemented hex string parsing of MultiUInt64

---
 include/openrave/openrave.h     | 59 +++++++++++++++++++++++++++++++++
 include/openrave/openravejson.h |  3 ++
 2 files changed, 62 insertions(+)

diff --git a/include/openrave/openrave.h b/include/openrave/openrave.h
index 56c4588ef1..d5d8bd4cc6 100644
--- a/include/openrave/openrave.h
+++ b/include/openrave/openrave.h
@@ -2605,6 +2605,37 @@ OPENRAVE_API void RaveGetVelocityFromAffineDOFVelocities(Vector& linearvel, Vect
 
 OPENRAVE_API ConfigurationSpecification RaveGetAffineConfigurationSpecification(int affinedofs,KinBodyConstPtr pbody=KinBodyConstPtr(),const std::string& interpolation="");
 
+/// \brief converts hex character to unsigned integer
+/// \param character to be converted into unsigned integer
+/// \return parsed unsigned integer
+inline uint64_t GetUIntFromHexChar(char character)
+{
+    if ('a' <= character && character <= 'f') {
+        return 10 + character - 'a';
+    }
+    if ('A' <= character && character <= 'F') {
+        return 10 + character - 'A';
+    }
+    if ('0' <= character && character <= '9') {
+        return  character - '0';
+    }
+    throw OPENRAVE_EXCEPTION_FORMAT0(("expected hex character and received \'" + std::string(1, character) + "\'"), ORE_InvalidArguments);
+}
+
+/// \brief implements std::stoull wiht sub string without memory allocation. Assumes string is hex-formatted.
+/// \param str string to parse from
+/// \param startPos position of the string to start parsing from
+/// \param length length to parse from the string
+/// \return parsed integer
+inline uint64_t ConvertHexSubStringToUint64(const char* str, size_t startPos, size_t length) {
+    uint64_t value(0);
+    for (size_t index = 0; index < length; ++index) {
+        const int stringPosition = startPos + index;
+        value += GetUIntFromHexChar(str[stringPosition]) << 4*(length - 1 - index);
+    }
+    return value;
+}
+
 template<std::size_t N>
 class OPENRAVE_API MultiUInt64
 {
@@ -2824,6 +2855,34 @@ class OPENRAVE_API MultiUInt64
         return ret;
     }
 
+    /// \brief loads from hex string.
+    /// \param hexStr hex-represented string of unsigned integer
+    void LoadFromHexString(const std::string& hexStr)
+    {
+        OPENRAVE_ASSERT(!hexStr.empty(), "Cannot convert empty string (\"%s\") to unsigned integer", hexStr, ORE_InvalidArguments);
+        // assume hex, 64bits can be represented by 16 chars
+        constexpr size_t lenOfuint64BitAsStr(16);
+
+        const size_t strLen = hexStr.size();
+        size_t startPosition = 0;
+        size_t endPosition = (strLen)%lenOfuint64BitAsStr;
+        if (endPosition == 0) {
+            endPosition = lenOfuint64BitAsStr;
+        }
+        for (size_t index64Bit = 0; index64Bit < N; ++index64Bit) {
+            if (startPosition >= strLen) {
+                break;
+            }
+            // MultiUInt64's MSB is index 0
+            const size_t index64BitWithOffset = (N - 1 - (strLen-1)/lenOfuint64BitAsStr) + index64Bit;
+            const uint64_t value = ConvertHexSubStringToUint64(hexStr.c_str(), startPosition, endPosition - startPosition);
+            SetValue(value, index64BitWithOffset);
+
+            startPosition = endPosition;
+            endPosition += lenOfuint64BitAsStr;
+        }
+    }
+
     static std::size_t GetNumUInt64() {
         return N;
     }
diff --git a/include/openrave/openravejson.h b/include/openrave/openravejson.h
index e64fa65468..18758a6380 100644
--- a/include/openrave/openravejson.h
+++ b/include/openrave/openravejson.h
@@ -327,6 +327,9 @@ inline void LoadJsonValue(const rapidjson::Value& v, MultiUInt64<N>& t)
             t.SetValue(value, index);
         }
     }
+    else if (v.IsString()) {
+        t.LoadFromString(v.GetString());
+    }
     else {
         throw OPENRAVE_EXCEPTION_FORMAT("Cannot convert JSON %s to MultiUInt64", GetJsonString(v), OpenRAVE::ORE_InvalidArguments);
     }