diff --git a/src/xml/xml_numeric_format.cc b/src/xml/xml_numeric_format.cc index 14840b53..31fb4813 100644 --- a/src/xml/xml_numeric_format.cc +++ b/src/xml/xml_numeric_format.cc @@ -17,14 +17,14 @@ namespace mujoco { namespace { - thread_local const char* precision = "%g"; + thread_local int precision = 6; } -const char* _mjPRIVATE__get_xml_precision() { +int _mjPRIVATE__get_xml_precision() { return precision; } -void _mjPRIVATE__set_xml_precision(const char* new_precision) { +void _mjPRIVATE__set_xml_precision(const int new_precision) { precision = new_precision; } diff --git a/src/xml/xml_numeric_format.h b/src/xml/xml_numeric_format.h index fdc37cc7..2efb9f10 100644 --- a/src/xml/xml_numeric_format.h +++ b/src/xml/xml_numeric_format.h @@ -20,15 +20,15 @@ namespace mujoco { extern "C" { - MJAPI const char* _mjPRIVATE__get_xml_precision(); - MJAPI void _mjPRIVATE__set_xml_precision(const char* precision); + MJAPI int _mjPRIVATE__get_xml_precision(); + MJAPI void _mjPRIVATE__set_xml_precision(const int precision); } // Full precision printing of floating point numbers in saved XMLs, useful for testing class FullFloatPrecision { public: - FullFloatPrecision() { _mjPRIVATE__set_xml_precision("%.17g");} - ~FullFloatPrecision() { _mjPRIVATE__set_xml_precision("%g");} + FullFloatPrecision() { _mjPRIVATE__set_xml_precision(17);} + ~FullFloatPrecision() { _mjPRIVATE__set_xml_precision(6);} }; } // namespace mujoco diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 3be5f937..5106b35d 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -19,8 +19,10 @@ #include #include #include +#include #include #include +#include #include #include "cc/array_safety.h" @@ -483,33 +485,16 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) { //---------------------------------- class mjXUtil implementation ---------------------------------- -// compare two vectors: double -bool mjXUtil::SameVector(const double* vec1, const double* vec2, int n) { +// compare two vectors +template +bool mjXUtil::SameVector(const T* vec1, const T* vec2, int n) { if (!vec1 || !vec2) { return false; } bool same = true; for (int i=0; i 1E-10) { - same = false; - } - } - - return same; -} - - - -// compare two vectors: double -bool mjXUtil::SameVector(const float* vec1, const float* vec2, int n) { - if (!vec1 || !vec2) { - return false; - } - - bool same = true; - for (int i=0; i 1E-7) { + if (fabs(vec1[i] - vec2[i]) > std::numeric_limits::epsilon()) { same = false; } } @@ -517,6 +502,10 @@ bool mjXUtil::SameVector(const float* vec1, const float* vec2, int n) { return same; } +template bool mjXUtil::SameVector(const double* vec1, const double* vec2, int n); +template bool mjXUtil::SameVector(const float* vec1, const float* vec2, int n); +template bool mjXUtil::SameVector(const int* vec1, const int* vec2, int n); +template bool mjXUtil::SameVector(const mjtByte* vec1, const mjtByte* vec2, int n); // find string in map, return corresponding integer (-1: not found) @@ -808,163 +797,60 @@ static int Round(double x) { } -// write attribute- double -void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, double* data, - const double* def) { - char buf[100]; - string value; - value.clear(); - +// write attribute +template +void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, T* data, const T* def) { // make sure all are defined - for (int i=0; i0) { - value = value + " "; - } - - // write integer or float - if (isint(data[i])) { - mju::sprintf_arr(buf, "%d", Round(data[i])); - } else { - mju::sprintf_arr(buf, mujoco::_mjPRIVATE__get_xml_precision(), data[i]); - } - - // append number - value = value + buf; - } - - // set attribute as string - WriteAttrTxt(elem, name, value); -} - - - -// write attribute- float -void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, float* data, - const float* def) { - char buf[100]; - string value; - value.clear(); - - // skip default attributes - if (SameVector(data, def, n)) { - return; - } - - // process all numbers - for (int i=0; i0) { - value = value + " "; - } - - // write integer or float - if (isint(data[i])) { - mju::sprintf_arr(buf, "%d", Round(data[i])); - } else { - mju::sprintf_arr(buf, "%g", data[i]); - } - - // append number - value = value + buf; - } - - // set attribute as string - WriteAttrTxt(elem, name, value); -} - - - -// write attribute- byte -void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, mjtByte* data, - const mjtByte* def) { - char buf[100]; - string value; - value.clear(); - - // skip default attributes - if (def) { - bool skip = true; - for (int i=0; i0) { - value = value + " "; - } - - // write integer - mju::sprintf_arr(buf, "%d", data[i]); - - // append number - value = value + buf; - } - - // set attribute as string - WriteAttrTxt(elem, name, value); -} - - -// write attribute- int -void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, int* data, - const int* def) { - char buf[100]; - string value; - value.clear(); - - // skip default attributes - if (def) { - bool skip = true; + if constexpr (std::is_floating_point_v) { for (int i=0; i0) { - value = value + " "; + stream << " "; } - // write integer - mju::sprintf_arr(buf, "%d", data[i]); - // append number - value = value + buf; + if (isint(data[i])) { + stream << Round(data[i]); + } else { + stream << data[i]; + } } // set attribute as string - WriteAttrTxt(elem, name, value); + WriteAttrTxt(elem, name, stream.str()); } +template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, + double* data, const double* def); + +template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, + float* data, const float* def); + +template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, + int* data, const int* def); + +template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, + mjtByte* data, const mjtByte* def); + + // write vector attribute, default = zero array void mjXUtil::WriteVector(XMLElement* elem, string name, vector& vec) { // proceed only if non-zero found diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index a1ee0b4b..91e4bb09 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -89,8 +89,8 @@ class mjXUtil { virtual ~mjXUtil() = default; // compare two vectors - static bool SameVector(const double* vec1, const double* vec2, int n); - static bool SameVector(const float* vec1, const float* vec2, int n); + template + static bool SameVector(const T* vec1, const T* vec2, int n); // find key in map, return value (-1: not found) static int FindKey(const mjMap* map, int mapsz, std::string key); @@ -136,21 +136,10 @@ class mjXUtil { static bool MapValue(tinyxml2::XMLElement* elem, const char* attr, int* data, const mjMap* map, int mapSz, bool required = false); - // write attribute- double - static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, double* data, - const double* def = 0); - - // write attribute- float - static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, float* data, - const float* def = 0); - - // write attribute- byte - static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, mjtByte* data, - const mjtByte* def = 0); - - // write attribute- int - static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, int* data, - const int* def = 0); + // write attribute- any type + template + static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, T* data, + const T* def = 0); // write vector attribute, with and without default static void WriteVector(tinyxml2::XMLElement* elem, std::string name, std::vector& vec);