diff --git a/CMakeModules/SimoxMacros.cmake b/CMakeModules/SimoxMacros.cmake index 41ed69a7f9e3684eaf233b2e21672cc03cd18e92..ed9a74ad832b1c5bd57be3aaced876cce6a3aed2 100644 --- a/CMakeModules/SimoxMacros.cmake +++ b/CMakeModules/SimoxMacros.cmake @@ -15,7 +15,7 @@ macro(setupSimoxExternalLibraries) FIND_PACKAGE(Qt4 4.6.0 COMPONENTS QtOpenGL QtCore QtGui) include(${QT_USE_FILE}) else() - FIND_PACKAGE(Qt5 5.5.0 COMPONENTS OpenGL Core Gui) + FIND_PACKAGE(Qt5 5.5.0 COMPONENTS OpenGL Core Gui Designer) endif() ENDIF() INCLUDE_DIRECTORIES(${Simox_INCLUDE_DIRS}) diff --git a/GraspPlanning/ApproachMovementGenerator.cpp b/GraspPlanning/ApproachMovementGenerator.cpp index 70aac39506651d2e1a527c8893d89e013e1a4d47..bb919b1e04a66e7b6a4f63ff4af4180c4020a25d 100644 --- a/GraspPlanning/ApproachMovementGenerator.cpp +++ b/GraspPlanning/ApproachMovementGenerator.cpp @@ -42,8 +42,7 @@ namespace GraspStudio } ApproachMovementGenerator::~ApproachMovementGenerator() - { - } + = default; VirtualRobot::RobotPtr ApproachMovementGenerator::getEEFRobotClone() diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.cpp b/GraspPlanning/ApproachMovementSurfaceNormal.cpp index 723deee3a0c7e4968ad5ef1e450eff877707508f..6e94dc6e66da54056e29bea37446a819710fbdd8 100644 --- a/GraspPlanning/ApproachMovementSurfaceNormal.cpp +++ b/GraspPlanning/ApproachMovementSurfaceNormal.cpp @@ -29,8 +29,7 @@ namespace GraspStudio } ApproachMovementSurfaceNormal::~ApproachMovementSurfaceNormal() - { - } + = default; bool ApproachMovementSurfaceNormal::getPositionOnObject(Eigen::Vector3f& storePos, Eigen::Vector3f& storeApproachDir) { diff --git a/GraspPlanning/CMakeLists.txt b/GraspPlanning/CMakeLists.txt index 7017369bc2fe1e659d6df660d6f677687336fb69..a8b50bee21b60835552b10e7a83d3d95a7fb8bf4 100644 --- a/GraspPlanning/CMakeLists.txt +++ b/GraspPlanning/CMakeLists.txt @@ -57,6 +57,7 @@ GraspPlanner/GenericGraspPlanner.cpp GraspQuality/GraspEvaluationPoseUncertainty.cpp GraspQuality/GraspQualityMeasure.cpp GraspQuality/GraspQualityMeasureWrenchSpace.cpp +GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp Visualization/ConvexHullVisualization.cpp ) @@ -73,6 +74,7 @@ GraspPlanner/GraspPlannerEvaluation.h GraspQuality/GraspEvaluationPoseUncertainty.h GraspQuality/GraspQualityMeasure.h GraspQuality/GraspQualityMeasureWrenchSpace.h +GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h Visualization/ConvexHullVisualization.h ) diff --git a/GraspPlanning/ContactConeGenerator.cpp b/GraspPlanning/ContactConeGenerator.cpp index b05581d5c14be8ec0c1ba488250c66200ca4338c..31e84c326e25bd7b9b8a7c12a2a7d2ed3ac0b579 100644 --- a/GraspPlanning/ContactConeGenerator.cpp +++ b/GraspPlanning/ContactConeGenerator.cpp @@ -11,9 +11,9 @@ // ************************************************************** #include "ContactConeGenerator.h" -#include <math.h> -#include <stdio.h> -#include <assert.h> +#include <cmath> +#include <cstdio> +#include <cassert> #include <iostream> #include <sstream> #include <fstream> diff --git a/GraspPlanning/ConvexHullGenerator.cpp b/GraspPlanning/ConvexHullGenerator.cpp index c176c2f712c682d6476cc26b143076754d93450e..a6bfd1dbe5737e34a87fdf10004706639c5bcaf4 100644 --- a/GraspPlanning/ConvexHullGenerator.cpp +++ b/GraspPlanning/ConvexHullGenerator.cpp @@ -1,9 +1,9 @@ #include "ConvexHullGenerator.h" #include <VirtualRobot/Visualization/TriMeshModel.h> -#include <math.h> +#include <cmath> #include <iostream> -#include <float.h> +#include <cfloat> //#define CONVEXHULL_DEBUG_OUTPUT @@ -127,7 +127,7 @@ namespace GraspStudio FILE* outfile = stdout; /* output from qh_produce_output()*/ /* use NULL to skip qh_produce_output() */ # else - FILE* outfile = NULL; /* output from qh_produce_output()*/ + FILE* outfile = nullptr; /* output from qh_produce_output()*/ /* use NULL to skip qh_produce_output() */ #endif FILE* errfile = stderr; /* error messages from qhull code */ @@ -159,11 +159,11 @@ namespace GraspStudio { facetT* facet_list = qh facet_list; //int convexNumFaces = qh num_facets; - /*int convexNumVert =*/ qh_setsize(qh_facetvertices(facet_list, NULL, false)); + /*int convexNumVert =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false)); qh_triangulate(); // need this for triangulated output! //int convexNumFaces2 = qh num_facets; - /*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, NULL, false)); + /*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false)); /* cout << "Numfacets1:" << convexNumFaces << endl; cout << "Numvertices1:" << convexNumVert << endl; @@ -183,9 +183,9 @@ namespace GraspStudio double pCenter[3]; - for (int u = 0; u < 3; u++) + for (double & u : pCenter) { - pCenter[u] = 0; + u = 0; } int nVcertexCount = 0; @@ -200,9 +200,9 @@ namespace GraspStudio } if (nVcertexCount > 0) - for (int u = 0; u < 3; u++) + for (double & u : pCenter) { - pCenter[u] /= (float)nVcertexCount; + u /= (float)nVcertexCount; } result->center[0] = pCenter[0]; @@ -319,7 +319,7 @@ namespace GraspStudio FILE* outfile = stdout; /* output from qh_produce_output()*/ /* use NULL to skip qh_produce_output() */ # else - FILE* outfile = NULL; /* output from qh_produce_output()*/ + FILE* outfile = nullptr; /* output from qh_produce_output()*/ /* use NULL to skip qh_produce_output() */ #endif FILE* errfile = stderr; /* error messages from qhull code */ @@ -353,23 +353,23 @@ namespace GraspStudio { facetT* facet_list = qh facet_list; //int convexNumFaces = qh num_facets; - /*int convexNumVert =*/ qh_setsize(qh_facetvertices(facet_list, NULL, false)); + /*int convexNumVert =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false)); qh_triangulate(); // need this for triangulated output! //int convexNumFaces2 = qh num_facets; - /*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, NULL, false)); + /*int convexNumVert2 =*/ qh_setsize(qh_facetvertices(facet_list, nullptr, false)); double pCenter[6]; - for (int u = 0; u < 6; u++) + for (double & u : pCenter) { - pCenter[u] = 0; + u = 0; } double pZero[6]; - for (int u = 0; u < 6; u++) + for (double & u : pZero) { - pZero[u] = 0; + u = 0; } int nVcertexCount = 0; @@ -384,9 +384,9 @@ namespace GraspStudio } if (nVcertexCount > 0) - for (int u = 0; u < 6; u++) + for (double & u : pCenter) { - pCenter[u] /= (float)nVcertexCount; + u /= (float)nVcertexCount; } result->center.p[0] = pCenter[0]; diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp index 8983008b0833b36f4d0c306786176fce65254767..b4753b4316ae92478e9dbed587db0e6ec87fe7ab 100644 --- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp +++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.cpp @@ -34,8 +34,7 @@ namespace GraspStudio } GenericGraspPlanner::~GenericGraspPlanner() - { - } + = default; int GenericGraspPlanner::plan(int nrGrasps, int timeOutMS, VirtualRobot::SceneObjectSetPtr obstacles) { diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.cpp b/GraspPlanning/GraspPlanner/GraspPlanner.cpp index aa7369158c43e4f30f1a2df5f020d3a4365cd332..86a12e21a326a984290afdf9ca629c9e9721d57d 100644 --- a/GraspPlanning/GraspPlanner/GraspPlanner.cpp +++ b/GraspPlanning/GraspPlanner/GraspPlanner.cpp @@ -9,9 +9,7 @@ namespace GraspStudio } GraspPlanner::~GraspPlanner() - { - - } + = default; void GraspPlanner::setVerbose(bool enable) { diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp index 7b14bff3ed0c8b5a241094625abec01cd7dd660c..b4b9720c60f795e3c1be45bc9c97c4b4db785446 100644 --- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp +++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp @@ -4,7 +4,7 @@ #include <algorithm> #include <random> -#include <float.h> +#include <cfloat> #include <cstdlib> using namespace Eigen; @@ -18,9 +18,7 @@ GraspEvaluationPoseUncertainty::GraspEvaluationPoseUncertainty(const PoseUncerta } GraspEvaluationPoseUncertainty::~GraspEvaluationPoseUncertainty() -{ - -} += default; std::vector<Eigen::Matrix4f> GraspEvaluationPoseUncertainty::generatePoses(const Eigen::Matrix4f &objectGP, const Eigen::Matrix4f &graspCenterGP) { @@ -244,18 +242,18 @@ GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty:: return res; res.numPosesTested = results.size(); - for (size_t i=0;i<results.size();i++) + for (auto & result : results) { - if (results.at(i).initialCollision) + if (result.initialCollision) { res.numColPoses++; } else { res.numValidPoses++; - res.avgQuality += results.at(i).quality; - res.avgQualityCol += results.at(i).quality; - if (results.at(i).forceClosure) + res.avgQuality += result.quality; + res.avgQualityCol += result.quality; + if (result.forceClosure) { res.forceClosureRate += 1.0f; res.forceClosureRateCol += 1.0f; diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp index 2b48150237a5f4b7b84d0d28368485cbfab26d11..6abbfeea8179b0f8dba15b860e73097838c73ba7 100644 --- a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp +++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp @@ -30,8 +30,7 @@ namespace GraspStudio } GraspQualityMeasure::~GraspQualityMeasure() - { - } + = default; bool GraspQualityMeasure::sampleObjectPoints(int nMaxFaces) { @@ -100,10 +99,10 @@ namespace GraspStudio return p; } - for (int i = 0; i < (int)sampledObjectPoints.size(); i++) + for (auto & sampledObjectPoint : sampledObjectPoints) { - p.p += sampledObjectPoints[i].p; - p.n += sampledObjectPoints[i].n; + p.p += sampledObjectPoint.p; + p.n += sampledObjectPoint.n; } p.p /= (float)sampledObjectPoints.size(); diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp index 060a40c04ecac741f793de1c61e181a7e202c9ef..86f4408faab7dfcb5d57edd81949097ffbacc808 100644 --- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp +++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.cpp @@ -18,7 +18,7 @@ #include <sstream> #include <fstream> #include <string> -#include <float.h> +#include <cfloat> // if defined, the inverted contact normals are used //#define INVERT_NORMALS @@ -39,8 +39,7 @@ namespace GraspStudio } GraspQualityMeasureWrenchSpace::~GraspQualityMeasureWrenchSpace() - { - } + = default; void GraspQualityMeasureWrenchSpace::setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints) @@ -296,10 +295,10 @@ namespace GraspStudio faceCenter.p.setZero(); faceCenter.n.setZero(); - for (int j = 0; j < 6; j++) + for (int j : faceIter->id) { - faceCenter.p += (convexHullGWS->vertices)[faceIter->id[j]].p; - faceCenter.n += (convexHullGWS->vertices)[faceIter->id[j]].n; + faceCenter.p += (convexHullGWS->vertices)[j].p; + faceCenter.n += (convexHullGWS->vertices)[j].n; } faceCenter.p /= 6.0f; @@ -406,15 +405,15 @@ namespace GraspStudio float fRes = FLT_MAX; int nWrongFacets = 0; - for (size_t i = 0; i < ch->faces.size(); i++) + for (auto & face : ch->faces) { - if (ch->faces[i].distNormCenter > 0) + if (face.distNormCenter > 0) { nWrongFacets++; } - else if (-(ch->faces[i].distNormCenter) < fRes) + else if (-(face.distNormCenter) < fRes) { - fRes = -(ch->faces[i].distNormCenter); + fRes = -(face.distNormCenter); } } diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7433974cb70652bd8836c9d7b3db07ac5ae9e6ac --- /dev/null +++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.cpp @@ -0,0 +1,362 @@ +// ************************************************************** +// Implementation of class GraspQualityMeasure +// ************************************************************** +// Author: Niko Vahrenkamp +// Date: 26.10.2011 +// ************************************************************** + + +// ************************************************************** +// includes +// ************************************************************** + +#include "GraspQualityMeasureWrenchSpaceNotNormalized.h" +#include <cmath> +#include <cstdio> +#include <cassert> +#include <iostream> +#include <sstream> +#include <fstream> +#include <string> +#include <cfloat> + + +using namespace std; +using namespace VirtualRobot; + +namespace GraspStudio +{ + + void GraspQualityMeasureWrenchSpaceNotNormalized::setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints) + { + GraspQualityMeasure::setContactPoints(contactPoints); + GWSCalculated = false; + } + + void GraspQualityMeasureWrenchSpaceNotNormalized::setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints) + { + GraspQualityMeasure::setContactPoints(contactPoints); + GWSCalculated = false; + } + + + float GraspQualityMeasureWrenchSpaceNotNormalized::getGraspQuality() + { + calculateGraspQuality(); + return graspQuality; + } + + bool GraspQualityMeasureWrenchSpaceNotNormalized::isGraspForceClosure() + { + updateGWS(); + return isOriginInGWSHull(); + } + + void GraspQualityMeasureWrenchSpaceNotNormalized::updateGWS() + { + if (GWSCalculated) + { + return; + } + bool printAll = false; + + if (contactPointsM.empty()) + { + if (verbose && printAll) + printf("Contact points not set.\n"); + return; + } + + //Rotate generic friction cone to align with object normals + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator objPointsIter; + std::vector<VirtualRobot::MathTools::ContactPoint> conePoints; + + if (verbose && printAll) + { + cout << "GWS contact points:" << endl; + } + + for (objPointsIter = contactPointsM.begin(); objPointsIter != contactPointsM.end(); objPointsIter++) + { + if (verbose && printAll) + { + MathTools::print((*objPointsIter)); + } + + coneGenerator->computeConePoints((*objPointsIter), conePoints); + } + + //Generate convex hull from rotated contact friction cones + convexHullGWS = calculateConvexHull(conePoints); + //calculateHullCenter(convexHullGWS, convexHullCenterGWS); + convexHullCenterGWS = convexHullGWS->center; + + if (verbose && printAll) + { + GRASPSTUDIO_INFO << " CENTER of GWS: " << endl; + MathTools::print(convexHullCenterGWS); + } + + GWSCalculated = true; + } + + VirtualRobot::MathTools::ConvexHull6DPtr GraspQualityMeasureWrenchSpaceNotNormalized::calculateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint>& points) + { + bool printAll = true; + + if (verbose && printAll) + { + cout << "Convex hull points for wrench calculation:" << endl; + printContacts(points); + } + + // create wrench + std::vector<VirtualRobot::MathTools::ContactPoint> wrenchPoints = createWrenchPoints(points, Eigen::Vector3f::Zero(), objectLength); // contact points are already moved so that com is at origin + + if (verbose && printAll) + { + cout << "Wrench points:" << endl; + printContacts(wrenchPoints); + } + + return ConvexHullGenerator::CreateConvexHull(wrenchPoints); + } + + std::vector<VirtualRobot::MathTools::ContactPoint> GraspQualityMeasureWrenchSpaceNotNormalized::createWrenchPoints(std::vector<VirtualRobot::MathTools::ContactPoint>& points, const Eigen::Vector3f& centerOfModel, float objectLengthMM) + { + std::vector<VirtualRobot::MathTools::ContactPoint> result; + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter = points.begin(); + VirtualRobot::MathTools::ContactPoint p; + Eigen::Vector3f normal; + + // convert objLength from mm to m + bool convertMM2M = true; + + float factor = 1.0f; + + if (objectLengthMM != 0) + { + if (convertMM2M) + { + factor = 2.0f / (objectLengthMM / 1000.0f); // == max distance from center ( 1 / (length/2) ) + } + else + { + factor = 2.0f / objectLengthMM; // == max distance from center ( 1 / (length/2) ) + } + } + + VirtualRobot::MathTools::ContactPoint tmpP; + + while (iter != points.end()) + { +#ifdef INVERT_NORMALS + /*p.p(0) = -(*iter).n(0); + p.p(1) = -(*iter).n(1); + p.p(2) = -(*iter).n(2);*/ + p.p = -1.0f * (iter->n); +#else + p.p = iter->n; +#endif + tmpP.p = iter->p - centerOfModel; + tmpP.n = -(iter->n); + + //normal = crossProductPosNormalInv(tmpP); + //p.n = factor * normal; + p.n = factor * tmpP.p.cross(tmpP.n); + + result.push_back(p); + iter++; + } + + return result; + } + + void GraspQualityMeasureWrenchSpaceNotNormalized::printContacts(std::vector<VirtualRobot::MathTools::ContactPoint>& points) + { + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter = points.begin(); + + while (iter != points.end()) + { + cout << "# "; + MathTools::print((*iter)); + iter++; + } + } + + VirtualRobot::MathTools::ContactPoint GraspQualityMeasureWrenchSpaceNotNormalized::calculateHullCenter(VirtualRobot::MathTools::ConvexHull6DPtr hull) + { + if (!hull) + { + GRASPSTUDIO_ERROR << "NULL data?!" << endl; + return VirtualRobot::MathTools::ContactPoint(); + } + + VirtualRobot::MathTools::ContactPoint resultCenter; + std::vector<VirtualRobot::MathTools::ContactPoint>::iterator iter; + resultCenter.p.setZero(); + resultCenter.n.setZero(); + + if (hull->vertices.size() == 0) + { + cout << __FUNCTION__ << ": error, no vertices..." << endl; + return resultCenter; + } + + for (iter = hull->vertices.begin(); iter != hull->vertices.end(); iter++) + { + resultCenter.p += iter->p; + resultCenter.n += iter->n; + } + + resultCenter.p /= (float)hull->vertices.size(); + resultCenter.n /= (float)hull->vertices.size(); + return resultCenter; + } + + float GraspQualityMeasureWrenchSpaceNotNormalized::minDistanceToGWSHull(VirtualRobot::MathTools::ContactPoint& point) + { + float minDist = FLT_MAX; + float dist[6]; + float currentDist2; + std::vector<MathTools::TriangleFace6D>::iterator faceIter; + + for (faceIter = convexHullGWS->faces.begin(); faceIter != convexHullGWS->faces.end(); faceIter++) + { + VirtualRobot::MathTools::ContactPoint faceCenter; + faceCenter.p.setZero(); + faceCenter.n.setZero(); + + for (int j : faceIter->id) + { + faceCenter.p += (convexHullGWS->vertices)[j].p; + faceCenter.n += (convexHullGWS->vertices)[j].n; + } + + faceCenter.p /= 6.0f; + faceCenter.n /= 6.0f; + + currentDist2 = 0; + + for (int j = 0; j < 3; j++) + { + dist[j] = (faceCenter.p(j) - point.p(j)); + dist[j + 3] = (faceCenter.n(j) - point.n(j)); + currentDist2 += dist[j] * dist[j]; + currentDist2 += dist[j + 3] * dist[j + 3]; + } + + if (currentDist2 < minDist) + { + minDist = currentDist2; + } + } + + return sqrtf(minDist); + } + bool GraspQualityMeasureWrenchSpaceNotNormalized::isOriginInGWSHull() + { + if (!GWSCalculated || !convexHullGWS) + { + return false; + } + + for (const auto& face : convexHullGWS->faces) + { + // ignore rounding errors + if (face.distPlaneZero > 1e-4) + { + return false; + } + } + + return true; + } + + + + float GraspQualityMeasureWrenchSpaceNotNormalized::minOffset(VirtualRobot::MathTools::ConvexHull6DPtr ch) + { + if (!ch) + { + return 0.0f; + } + + float fRes = FLT_MAX; + int nWrongFacets = 0; + + for (auto & face : ch->faces) + { + const auto dist = face.distNormCenter; + if (dist > 0) + { + //outside + nWrongFacets++; + continue; + } + fRes = std::max(fRes, -dist); + } + + if (nWrongFacets > 0) + { + cout << __FUNCTION__ << " Warning: offset of " << nWrongFacets << " facets >0 (# of facets:" << ch->faces.size() << ")" << endl; + } + + return fRes; + } + + float GraspQualityMeasureWrenchSpaceNotNormalized::getVolumeGraspMeasure() + { + updateGWS(); + + if (!convexHullGWS || convexHullGWS->vertices.size() == 0) + { + cout << __FUNCTION__ << "No vertices in Grasp Wrench Space?! Maybe I was not initialized correctly..." << endl; + return 0.0; + } + return convexHullGWS->volume; + } + + bool GraspQualityMeasureWrenchSpaceNotNormalized::calculateGraspQuality() + { + updateGWS(); + + graspQuality = 0.0f; + + if (!convexHullGWS || convexHullGWS->vertices.size() == 0) + { + cout << __FUNCTION__ << "No vertices in Grasp Wrench Space?! Maybe I was not initialized correctly..." << endl; + return 0.0; + } + + float fResOffsetGWS = minOffset(convexHullGWS); + + graspQuality = fResOffsetGWS; + + if (verbose) + { + GRASPSTUDIO_INFO << endl; + cout << ": GWS volume : " << convexHullGWS->volume << endl; + cout << ": GWS min Offset: " << fResOffsetGWS << endl; + cout << ": GraspQuality : " << graspQuality << endl; + } + + return true; + } + + std::string GraspQualityMeasureWrenchSpaceNotNormalized::getName() + { + return "GraspWrenchSpaceNotNormalized"; + } + + + Eigen::Vector3f GraspQualityMeasureWrenchSpaceNotNormalized::crossProductPosNormalInv(const VirtualRobot::MathTools::ContactPoint& v1) + { + Eigen::Vector3f res; + res(0) = v1.p(1) * (-v1.n(2)) - v1.p(2) * (-v1.n(1)); + res(1) = v1.p(2) * (-v1.n(0)) - v1.p(0) * (-v1.n(2)); + res(2) = v1.p(0) * (-v1.n(1)) - v1.p(1) * (-v1.n(0)); + return res; + } + +} diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h new file mode 100644 index 0000000000000000000000000000000000000000..a26b121c23a2fcbf494391fc0d74a494a0c611ce --- /dev/null +++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpaceNotNormalized.h @@ -0,0 +1,128 @@ +/** +* This file is part of Simox. +* +* Simox is free software; you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as +* published by the Free Software Foundation; either version 2 of +* the License, or (at your option) any later version. +* +* Simox is distributed in the hope that it will be useful, but +* WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see <http://www.gnu.org/licenses/>. +* +* @package GraspStudio +* @author Nikolaus Vahrenkamp +* @copyright 2011 Nikolaus Vahrenkamp +* GNU Lesser General Public License +* +*/ +#pragma once + +#include "../GraspStudio.h" +#include "GraspQualityMeasure.h" + +#include <Eigen/Core> + +namespace GraspStudio +{ + + /*! + \brief An efficient implementation of the grasp wrench space algorithm for grasp quality evaluation. + + the grasp wrench space algorithm is widely used in the context of grasp planning. By analyzing + the grasp wrench space (GWS) of a given set of contact points, a quality score of a grasp + can be evaluated. + In this implementation, additionally an object specific wrench space (WS) is calculated, which + approximatevly represents a "perfect" grasp. This object is used to normalize the quality score. + */ + class GRASPSTUDIO_IMPORT_EXPORT GraspQualityMeasureWrenchSpaceNotNormalized : public GraspQualityMeasure + { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + //inherit ctor + using GraspQualityMeasure::GraspQualityMeasure; + + + //! This method is used to compute a reference value that describes a perfect grasp + bool calculateObjectProperties() override { return true; } + + /*! + Returns f_max_gws + with f_max_gws = max distance of GWS hull center to one of its facets + -> also known as "epsilon" quality == radius of larges enclosing 6D ball + */ + float getGraspQuality() override; + + /*! + Volume grasp quality ratio of GWS volume + -> also known as "v" quality + */ + virtual float getVolumeGraspMeasure(); + + /* + Checks if wrench space origin is inside GWS-Hull + */ + bool isGraspForceClosure() override; + + /* + Returns the internally calculated convex hull object (GraspWrenchSpace) + This hull depends on the contacts + */ + virtual VirtualRobot::MathTools::ConvexHull6DPtr getConvexHullGWS() + { + return convexHullGWS; + } + + void updateGWS(); + + VirtualRobot::MathTools::ContactPoint getCenterGWS() + { + return convexHullCenterGWS; + } + + /*! + setup contact information + the contact points are normalized by subtracting the COM + the contact normals are normalize to unit length + */ + void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints) override; + void setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints) override; + + bool calculateGraspQuality() override; + + //! Returns description of this object + std::string getName() override; + + + static std::vector<VirtualRobot::MathTools::ContactPoint> createWrenchPoints(std::vector<VirtualRobot::MathTools::ContactPoint>& points, const Eigen::Vector3f& centerOfModel, float objectLengthMM); + + //! Goes through all facets of convex hull and searches the minimum distance to it's center + static float minOffset(VirtualRobot::MathTools::ConvexHull6DPtr ch); + + protected: + + //Methods + VirtualRobot::MathTools::ConvexHull6DPtr calculateConvexHull(std::vector<VirtualRobot::MathTools::ContactPoint>& points); + VirtualRobot::MathTools::ContactPoint calculateHullCenter(VirtualRobot::MathTools::ConvexHull6DPtr hull); + + float minDistanceToGWSHull(VirtualRobot::MathTools::ContactPoint& point); + + + bool isOriginInGWSHull(); + void printContacts(std::vector<VirtualRobot::MathTools::ContactPoint>& points); + static Eigen::Vector3f crossProductPosNormalInv(const VirtualRobot::MathTools::ContactPoint& v1); + + //For safety + bool GWSCalculated {false}; + + //For Object and Grasp Wrench Space Calculation + VirtualRobot::MathTools::ConvexHull6DPtr convexHullGWS; + VirtualRobot::MathTools::ContactPoint convexHullCenterGWS; + }; +} + diff --git a/GraspPlanning/GraspStudio.h b/GraspPlanning/GraspStudio.h index 7cf38174fae824250ef60703bd8e1f1f5a169f5e..1d6de55a64e0a115dca56580d1101a5134e97f02 100644 --- a/GraspPlanning/GraspStudio.h +++ b/GraspPlanning/GraspStudio.h @@ -73,6 +73,7 @@ namespace GraspStudio class GraspQualityMeasure; class GraspQualityMeasureWrenchSpace; + class GraspQualityMeasureWrenchSpaceNotNormalized; class ContactConeGenerator; class GraspQualityMeasure; @@ -83,6 +84,7 @@ namespace GraspStudio typedef boost::shared_ptr<GraspQualityMeasure> GraspQualityMeasurePtr; typedef boost::shared_ptr<GraspQualityMeasureWrenchSpace> GraspQualityMeasureWrenchSpacePtr; + typedef boost::shared_ptr<GraspQualityMeasureWrenchSpaceNotNormalized> GraspQualityMeasureWrenchSpaceNotNormalizedPtr; typedef boost::shared_ptr<ContactConeGenerator> ContactConeGeneratorPtr; typedef boost::shared_ptr<GraspQualityMeasure> GraspQualityMeasurePtr; typedef boost::shared_ptr<ApproachMovementGenerator> ApproachMovementGeneratorPtr; diff --git a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp index 5ae6bf4e5129bda59567db8fb6354890640d92f5..51ccb15fddd6e68a262cb159793d44271e120ce0 100644 --- a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp +++ b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.cpp @@ -215,17 +215,17 @@ namespace GraspStudio for (int i = 0; i < nFaces; i++) { - for (int j = 0; j < 6; j++) + for (int j : convHull->faces[i].id) { if (buseFirst3Coords) { //v[j] = convHull->vertices.at(convHull->faces[i].id[j]).p; - vProjectedPoints.push_back(convHull->vertices[ convHull->faces[i].id[j] ].p); + vProjectedPoints.push_back(convHull->vertices[ j ].p); } else { //v[j] = convHull->vertices.at(convHull->faces[i].id[j]).n; - vProjectedPoints.push_back(convHull->vertices[ convHull->faces[i].id[j] ].n); + vProjectedPoints.push_back(convHull->vertices[ j ].n); } } diff --git a/GraspPlanning/Visualization/ConvexHullVisualization.cpp b/GraspPlanning/Visualization/ConvexHullVisualization.cpp index 13a0a174c7b120504db255b8fa291c7536c9917d..9247ea7b836b737669f0afba6f6c64ff99315067 100644 --- a/GraspPlanning/Visualization/ConvexHullVisualization.cpp +++ b/GraspPlanning/Visualization/ConvexHullVisualization.cpp @@ -20,7 +20,6 @@ namespace GraspStudio ConvexHullVisualization::~ConvexHullVisualization() - { - } + = default; } // namespace GraspStudio diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp index 871f76570a448e96d3357b19f7463a82d505180a..29eff9aea79113e2d4a7f45c532681144a6122ee 100644 --- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp +++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp @@ -21,7 +21,7 @@ #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -43,7 +43,7 @@ using namespace GraspStudio; float TIMER_MS = 30.0f; GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefName, std::string& preshape, std::string& objFile) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -52,7 +52,7 @@ GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefNam this->objectFile = objFile; this->eefName = eefName; this->preshape = preshape; - eefVisu = NULL; + eefVisu = nullptr; sceneSep = new SoSeparator; sceneSep->ref(); @@ -262,15 +262,15 @@ void GraspPlannerWindow::buildVisu() } // add approach dir visu - for (size_t i = 0; i < contacts.size(); i++) + for (auto & contact : contacts) { SoSeparator* s = new SoSeparator; Eigen::Matrix4f ma; ma.setIdentity(); - ma.block(0, 3, 3, 1) = contacts[i].contactPointFingerGlobal; + ma.block(0, 3, 3, 1) = contact.contactPointFingerGlobal; SoMatrixTransform* m = CoinVisualizationFactory::getMatrixTransformScaleMM2M(ma); s->addChild(m); - s->addChild(CoinVisualizationFactory::CreateArrow(contacts[i].approachDirectionGlobal, 10.0f, 1.0f)); + s->addChild(CoinVisualizationFactory::CreateArrow(contact.approachDirectionGlobal, 10.0f, 1.0f)); frictionConeSep->addChild(s); } } diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp index 744b1cf4c726cbc9b8e14bfdacdad6e6b6b40aee..3afdf57930ea0633e041959e4159511719cb8972 100644 --- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp +++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp @@ -16,7 +16,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -38,7 +38,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; GraspQualityWindow::GraspQualityWindow(std::string& robFile, std::string& objFile) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -488,9 +488,9 @@ void GraspQualityWindow::setEEFComboBox() robot->getEndEffectors(eefs); - for (size_t i = 0; i < eefs.size(); i++) + for (auto & eef : eefs) { - QString nameEEF(eefs[i]->getName().c_str()); + QString nameEEF(eef->getName().c_str()); UI.comboBoxEEF->addItem(nameEEF); } } diff --git a/MotionPlanning/ApproachDiscretization.cpp b/MotionPlanning/ApproachDiscretization.cpp index 425d8883b83eb0f37dcf790ef2e11034a03b37d3..e3d399c4249ca6642211cc67c9989f178cd08a22 100644 --- a/MotionPlanning/ApproachDiscretization.cpp +++ b/MotionPlanning/ApproachDiscretization.cpp @@ -1,8 +1,8 @@ #include "ApproachDiscretization.h" #include <iostream> -#include <time.h> +#include <ctime> #include <algorithm> -#include <limits.h> +#include <climits> using namespace std; @@ -24,8 +24,7 @@ namespace Saba } ApproachDiscretization::~ApproachDiscretization() - { - } + = default; /* void ApproachDiscretization::buildIVModel() @@ -93,15 +92,15 @@ namespace Saba zeroPt.setZero(); VirtualRobot::SphereApproximator::FaceIndex faceIndx = sphere.mapVerticeIndxToFaceIndx[nIndex]; - for (int i = 0; i < (int)faceIndx.faceIds.size(); i++) + for (int faceId : faceIndx.faceIds) { - VirtualRobot::MathTools::TriangleFace f = sphere.faces[faceIndx.faceIds[i]]; + VirtualRobot::MathTools::TriangleFace f = sphere.faces[faceId]; //if (sphereGenerator->check_intersect_tri(sphere.vertices[f.n1],sphere.m_Vertices[f.m_n2],sphere.m_Vertices[f.m_n3],zeroPt,Pose1,storeIntPoint)) if (sphereGenerator->check_intersect_tri(sphere.vertices[f.id1], sphere.vertices[f.id2], sphere.vertices[f.id3], zeroPt, dir, storeIntPoint)) { //MarkFace(FaceIndx.m_faceIds[i],true); - return faceIndx.faceIds[i]; + return faceId; } } diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp index 5c4ff7df7e700bfdab6802e4cb56aef7ea45db29..40a496b56cfa381089e7739f5f842b3867c64dd4 100644 --- a/MotionPlanning/CSpace/CSpace.cpp +++ b/MotionPlanning/CSpace/CSpace.cpp @@ -8,11 +8,11 @@ #include "ConfigurationConstraint.h" #include <VirtualRobot/CollisionDetection/CDManager.h> #include <VirtualRobot/CollisionDetection/CollisionChecker.h> -#include "float.h" +#include <cfloat> #include <cmath> #include <fstream> #include <iomanip> -#include <time.h> +#include <ctime> #include <VirtualRobot/MathTools.h> #include <VirtualRobot/Random.h> #define GET_RANDOM_DATA_FROM_64BIT_ADDRESS(a) (int)(0xFF & (long)a) | (0xFF00 & ((long)a >> 16)) @@ -842,9 +842,9 @@ namespace Saba bool CSpace::isSatisfyingConstraints(const Eigen::VectorXf& config) { - for (size_t i = 0; i < constraints.size(); i++) + for (auto & constraint : constraints) { - if (!constraints[i]->isValid(config)) + if (!constraint->isValid(config)) { return false; } diff --git a/MotionPlanning/CSpace/CSpaceNode.cpp b/MotionPlanning/CSpace/CSpaceNode.cpp index 6d35b3a092716ee060dd12b78551917b3040d99a..faab447bd30a6355f5dfaec71e7f2339793f5729 100644 --- a/MotionPlanning/CSpace/CSpaceNode.cpp +++ b/MotionPlanning/CSpace/CSpaceNode.cpp @@ -5,11 +5,9 @@ namespace Saba { CSpaceNode::CSpaceNode() - { - } + = default; CSpaceNode::~CSpaceNode() - { - } + = default; } diff --git a/MotionPlanning/CSpace/CSpacePath.cpp b/MotionPlanning/CSpace/CSpacePath.cpp index 22edc4a40020b8d802c073809ab80db94371d96c..411d94721f076291c0b08a44f9c6047f62181e2b 100644 --- a/MotionPlanning/CSpace/CSpacePath.cpp +++ b/MotionPlanning/CSpace/CSpacePath.cpp @@ -318,7 +318,7 @@ namespace Saba { storePathPos = getPoint(0); - if (storeIndex != NULL) + if (storeIndex != nullptr) { *storeIndex = 0; } @@ -329,7 +329,7 @@ namespace Saba { storePathPos = getPoint(getNrOfPoints() - 1); - if (storeIndex != NULL) + if (storeIndex != nullptr) { *storeIndex = (int)path.size(); } @@ -400,7 +400,7 @@ namespace Saba } } - if (storeIndex != NULL) + if (storeIndex != nullptr) { *storeIndex = startIndex; } diff --git a/MotionPlanning/CSpace/CSpaceSampled.cpp b/MotionPlanning/CSpace/CSpaceSampled.cpp index 91c7119d0584258be77cd389ea3410f660a7c96b..3ec07330c7411f8cd971f8cd5f60c8e3277e0df0 100644 --- a/MotionPlanning/CSpace/CSpaceSampled.cpp +++ b/MotionPlanning/CSpace/CSpaceSampled.cpp @@ -6,7 +6,7 @@ #include "VirtualRobot/Robot.h" //#include "MathHelpers.h" #include <cmath> -#include "float.h" +#include <cfloat> #include <iostream> #include <string> @@ -38,8 +38,7 @@ namespace Saba CSpaceSampled::~CSpaceSampled() - { - } + = default; CSpacePtr CSpaceSampled::clone(VirtualRobot::CollisionCheckerPtr newColChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int newRandomSeed) { diff --git a/MotionPlanning/CSpace/CSpaceTree.cpp b/MotionPlanning/CSpace/CSpaceTree.cpp index 8e2a632b2ce2bec7998c854b4a56bf59d63e7170..44b6fb543d15b3830eb7a4a5a269f828c022edd4 100644 --- a/MotionPlanning/CSpace/CSpaceTree.cpp +++ b/MotionPlanning/CSpace/CSpaceTree.cpp @@ -6,11 +6,11 @@ #include "CSpace.h" #include "VirtualRobot/Robot.h" #include "VirtualRobot/RobotNodeSet.h" -#include "float.h" +#include <cfloat> #include <cmath> #include <fstream> #include <iomanip> -#include <time.h> +#include <ctime> using namespace std; @@ -41,9 +41,7 @@ namespace Saba } CSpaceTree::~CSpaceTree() - { - - } + = default; void CSpaceTree::lock() { @@ -323,7 +321,7 @@ namespace Saba } } - if (storeDist != NULL) + if (storeDist != nullptr) { *storeDist = sqrtf(dist2); } @@ -362,10 +360,10 @@ namespace Saba CSpaceNodePtr actualNode; - for (unsigned int i = 0; i < nodes.size(); i++) + for (const auto & node : nodes) { // save ID, configuration and parentID of each node in one row - actualNode = nodes[i]; + actualNode = node; file << actualNode->ID << " "; for (unsigned int j = 0; j < dimension; j++) diff --git a/MotionPlanning/CSpace/ConfigurationConstraint.cpp b/MotionPlanning/CSpace/ConfigurationConstraint.cpp index 766e5d176e0d15c55030b6e1dd2394de2ea9a35f..fd34b5182619ae036ecaf995cc5262e7c965b927 100644 --- a/MotionPlanning/CSpace/ConfigurationConstraint.cpp +++ b/MotionPlanning/CSpace/ConfigurationConstraint.cpp @@ -10,7 +10,6 @@ namespace Saba } ConfigurationConstraint::~ConfigurationConstraint() - { - } + = default; } diff --git a/MotionPlanning/CSpace/Sampler.cpp b/MotionPlanning/CSpace/Sampler.cpp index e38260743aa988c8734b718e756688d876aa61a6..2ce3810908670678767258ce2360c81c7c19f3f5 100644 --- a/MotionPlanning/CSpace/Sampler.cpp +++ b/MotionPlanning/CSpace/Sampler.cpp @@ -11,8 +11,7 @@ namespace Saba } Sampler::~Sampler() - { - } + = default; void Sampler::getUniformlyRandomConfig(Eigen::VectorXf& stroreConfig, CSpacePtr space) { diff --git a/MotionPlanning/Planner/BiRrt.cpp b/MotionPlanning/Planner/BiRrt.cpp index 79188b0cbfbd31ac8069439fe414ba9bac316453..aa666c2f2ff0e337bab353388caca7e9a892ee20 100644 --- a/MotionPlanning/Planner/BiRrt.cpp +++ b/MotionPlanning/Planner/BiRrt.cpp @@ -5,7 +5,7 @@ #include "../CSpace/CSpaceTree.h" #include "../CSpace/CSpacePath.h" #include "VirtualRobot/Robot.h" -#include <time.h> +#include <ctime> //#define LOCAL_DEBUG(a) {SABA_INFO << a;}; @@ -26,8 +26,7 @@ namespace Saba } BiRrt::~BiRrt() - { - } + = default; bool BiRrt::plan(bool bQuiet) @@ -353,14 +352,14 @@ namespace Saba // Create the CSpacePath, first start node till bridgeover node, then bridgeover node till goal node solution.reset(new CSpacePath(cspace)); - for (unsigned int i = 0; i < tmpSol.size(); i++) + for (int i : tmpSol) { - solution->addPoint(tree->getNode(tmpSol[i])->configuration); + solution->addPoint(tree->getNode(i)->configuration); } - for (unsigned int i = 0; i < tmpSol2.size(); i++) + for (int i : tmpSol2) { - solution->addPoint(tree2->getNode(tmpSol2[i])->configuration); + solution->addPoint(tree2->getNode(i)->configuration); } if (!bQuiet) diff --git a/MotionPlanning/Planner/GraspIkRrt.cpp b/MotionPlanning/Planner/GraspIkRrt.cpp index df370ecca466e0e2847199f3f350ed0b54aeece1..2c6b1fdb8b95bcda456accd67cad69ec87d5ae22 100644 --- a/MotionPlanning/Planner/GraspIkRrt.cpp +++ b/MotionPlanning/Planner/GraspIkRrt.cpp @@ -8,7 +8,7 @@ #include <VirtualRobot/RobotNodeSet.h> #include <VirtualRobot/Grasping/GraspSet.h> #include <VirtualRobot/Random.h> -#include <time.h> +#include <ctime> //#define LOCAL_DEBUG(a) {SABA_INFO << a;}; @@ -49,8 +49,7 @@ namespace Saba } GraspIkRrt::~GraspIkRrt() - { - } + = default; bool GraspIkRrt::searchNewGoal() { diff --git a/MotionPlanning/Planner/GraspRrt.cpp b/MotionPlanning/Planner/GraspRrt.cpp index 4c56eff29f1a8d27159cccf3557b1532c88436ee..48b6a1d5e9c6200baafe98219d96c6bfb4d6794c 100644 --- a/MotionPlanning/Planner/GraspRrt.cpp +++ b/MotionPlanning/Planner/GraspRrt.cpp @@ -9,8 +9,8 @@ #include <VirtualRobot/Grasping/BasicGraspQualityMeasure.h> #include <VirtualRobot/Random.h> #include <algorithm> -#include <float.h> -#include <time.h> +#include <cfloat> +#include <ctime> using namespace std; @@ -80,9 +80,7 @@ namespace Saba } GraspRrt::~GraspRrt() - { - - } + = default; void GraspRrt::reset() @@ -912,11 +910,11 @@ namespace Saba VirtualRobot::EndEffector::ContactInfoVector contacts; // we only need the targetObject contacts - for (size_t i = 0; i < contactsAll.size(); i++) + for (auto & i : contactsAll) { - if (contactsAll[i].obstacle == targetObject) + if (i.obstacle == targetObject) { - contacts.push_back(contactsAll[i]); + contacts.push_back(i); } } @@ -929,9 +927,9 @@ namespace Saba cout << __FUNCTION__ << ": Low number of contacts -> Zero Grasp Score " << endl; cout << "Fingers: " ; - for (int i = 0; i < (int)contacts.size(); i++) + for (auto & contact : contacts) { - cout << contacts[i].actor->getName() << ", "; + cout << contact.actor->getName() << ", "; } cout << endl; diff --git a/MotionPlanning/Planner/PlanningThread.cpp b/MotionPlanning/Planner/PlanningThread.cpp index a292cdf69bde32e5c4cc0f5d928bdf277e957f30..18365c3dd4c87a06c8844490ae871c424472e124 100644 --- a/MotionPlanning/Planner/PlanningThread.cpp +++ b/MotionPlanning/Planner/PlanningThread.cpp @@ -1,7 +1,7 @@ #include "PlanningThread.h" -#include <time.h> +#include <ctime> #include <iostream> using namespace std; diff --git a/MotionPlanning/Planner/Rrt.cpp b/MotionPlanning/Planner/Rrt.cpp index 7a21baa3b43f738a996798257340f1de1dfecfd7..1c5ab1e6b58d4b6b5656b94e11f840b8ad27f935 100644 --- a/MotionPlanning/Planner/Rrt.cpp +++ b/MotionPlanning/Planner/Rrt.cpp @@ -5,7 +5,7 @@ #include "../CSpace/CSpacePath.h" #include "VirtualRobot/Robot.h" #include <VirtualRobot/Random.h> -#include <time.h> +#include <ctime> using namespace std; @@ -44,8 +44,7 @@ namespace Saba } Rrt::~Rrt() - { - } + = default; bool Rrt::plan(bool bQuiet) diff --git a/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp b/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp index b7d24253dd2815edf5e683ab553a1ae16d39e16d..1596a847bed86c52532bdaf1d0f0d270189047ab 100644 --- a/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp +++ b/MotionPlanning/PostProcessing/ElasticBandProcessor.cpp @@ -3,8 +3,8 @@ #include "MotionPlanning/CSpace/CSpaceSampled.h" #include "MotionPlanning/CSpace/CSpacePath.h" #include <vector> -#include <time.h> -#include <math.h> +#include <ctime> +#include <cmath> namespace Saba { @@ -45,8 +45,7 @@ using namespace VirtualRobot; } ElasticBandProcessor::~ElasticBandProcessor() - { - } + = default; bool ElasticBandProcessor::initSolution() { diff --git a/MotionPlanning/PostProcessing/PathProcessingThread.cpp b/MotionPlanning/PostProcessing/PathProcessingThread.cpp index 90b784d3c3567859328006102c6f929608ce1112..24171680f5603bb305af6343da60a14145423958 100644 --- a/MotionPlanning/PostProcessing/PathProcessingThread.cpp +++ b/MotionPlanning/PostProcessing/PathProcessingThread.cpp @@ -4,7 +4,7 @@ #include "PathProcessingThread.h" -#include <time.h> +#include <ctime> #include <iostream> using namespace std; diff --git a/MotionPlanning/PostProcessing/PathProcessor.cpp b/MotionPlanning/PostProcessing/PathProcessor.cpp index 7df93c8192112331a577a0655e671da7ba0d89b0..56d911c55a4159ee76ac5fb738e3be1d8a123bbf 100644 --- a/MotionPlanning/PostProcessing/PathProcessor.cpp +++ b/MotionPlanning/PostProcessing/PathProcessor.cpp @@ -13,8 +13,7 @@ namespace Saba } PathProcessor::~PathProcessor() - { - } + = default; CSpacePathPtr PathProcessor::getOptimizedPath() { diff --git a/MotionPlanning/PostProcessing/ShortcutProcessor.cpp b/MotionPlanning/PostProcessing/ShortcutProcessor.cpp index 94e0e817d920f9b42aa2a1fab41ab579e1a97ae6..665932febc97a236493a914d5f1f6e40af910f22 100644 --- a/MotionPlanning/PostProcessing/ShortcutProcessor.cpp +++ b/MotionPlanning/PostProcessing/ShortcutProcessor.cpp @@ -3,8 +3,8 @@ #include "MotionPlanning/CSpace/CSpaceSampled.h" #include "MotionPlanning/CSpace/CSpacePath.h" #include <vector> -#include <time.h> -#include <math.h> +#include <ctime> +#include <cmath> namespace Saba { @@ -15,8 +15,7 @@ namespace Saba } ShortcutProcessor::~ShortcutProcessor() - { - } + = default; diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp index 507c8d6b4e27024ca19c55484e37138ecd7474f3..a84d55c430db5a74c4534df3b0cf31d045d9fe01 100644 --- a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp +++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.cpp @@ -30,14 +30,14 @@ namespace Saba CoinRrtWorkspaceVisualization::CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string& TCPName) : RrtWorkspaceVisualization(robot, cspace, TCPName) { - visualization = NULL; + visualization = nullptr; coinInit(); } CoinRrtWorkspaceVisualization::CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string& TCPName) : RrtWorkspaceVisualization(robot, robotNodeSet, TCPName) { - visualization = NULL; + visualization = nullptr; coinInit(); } @@ -262,9 +262,9 @@ namespace Saba Eigen::Vector3f p; Eigen::Vector3f p2; - for (unsigned int i = 0; i < nodes.size(); i++) + for (const auto & node : nodes) { - actualNode = nodes[i]; + actualNode = node; // get tcp coords: robot->setJointValues(robotNodeSet, actualNode->configuration); @@ -278,10 +278,10 @@ namespace Saba } - for (unsigned int i = 0; i < nodes.size(); i++) + for (const auto & node : nodes) { - actualNode = nodes[i]; + actualNode = node; parentid = actualNode->parentID; // create 3D model for nodes diff --git a/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp b/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp index a91354f7141f3a534f5deafa3066b9f43a344d4b..82cbb5eb56e7867e0a733bc3aaa7c71c550c15e4 100644 --- a/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp +++ b/MotionPlanning/Visualization/RrtWorkspaceVisualization.cpp @@ -31,8 +31,7 @@ namespace Saba } RrtWorkspaceVisualization::~RrtWorkspaceVisualization() - { - } + = default; void RrtWorkspaceVisualization::init() { diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp index 4d0af86905c0cac2d7c6d281428384a59d1b1478..06a05c5b1809f3e365b325660a268218f0fadc52 100644 --- a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp +++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp @@ -18,7 +18,7 @@ #include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h> #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -40,7 +40,7 @@ GraspRrtWindow::GraspRrtWindow(const std::string& sceneFile, const std::string& const std::string& rns, const std::string& rnsB, const std::string& eefName, const std::string& eefNameB, const std::string& colModelRob1, const std::string& colModelRob1B, const std::string& colModelRob2, const std::string& colModelRob2B, const std::string& colModelEnv) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -196,7 +196,7 @@ void GraspRrtWindow::buildVisu() if (scene) { visualization = scene->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -215,10 +215,10 @@ void GraspRrtWindow::buildVisu() { SoSeparator* eefVisu = CoinVisualizationFactory::CreateEndEffectorVisualization(eef); - for (size_t i = 0; i < grasps.size(); i++) + for (auto & grasp : grasps) { - Eigen::Matrix4f m = grasps[i]->getTcpPoseGlobal(targetObject->getGlobalPose()); + Eigen::Matrix4f m = grasp->getTcpPoseGlobal(targetObject->getGlobalPose()); SoSeparator* sep1 = new SoSeparator(); SoMatrixTransform* mt = CoinVisualizationFactory::getMatrixTransformScaleMM2M(m); sep1->addChild(mt); @@ -309,9 +309,9 @@ void GraspRrtWindow::loadScene() UI.comboBoxStart->clear(); - for (size_t i = 0; i < configs.size(); i++) + for (auto & config : configs) { - QString qtext = configs[i]->getName().c_str(); + QString qtext = config->getName().c_str(); UI.comboBoxStart->addItem(qtext); } @@ -330,9 +330,9 @@ void GraspRrtWindow::loadScene() UI.comboBoxGoal->clear(); - for (size_t i = 0; i < obstacles.size(); i++) + for (auto & obstacle : obstacles) { - QString qtext = obstacles[i]->getName().c_str(); + QString qtext = obstacle->getName().c_str(); UI.comboBoxGoal->addItem(qtext); } @@ -345,9 +345,9 @@ void GraspRrtWindow::loadScene() UI.comboBoxColModelEnv->clear(); QString qtext; - for (size_t i = 0; i < soss.size(); i++) + for (auto & sos : soss) { - qtext = soss[i]->getName().c_str(); + qtext = sos->getName().c_str(); UI.comboBoxColModelEnv->addItem(qtext); } @@ -384,9 +384,9 @@ void GraspRrtWindow::loadScene() UI.comboBoxColModelRobotStatic->clear(); UI.comboBoxRNS->clear(); - for (size_t i = 0; i < rnss.size(); i++) + for (auto & rns : rnss) { - qtext = rnss[i]->getName().c_str(); + qtext = rns->getName().c_str(); UI.comboBoxColModelRobot->addItem(qtext); UI.comboBoxColModelRobotStatic->addItem(qtext); UI.comboBoxRNS->addItem(qtext); diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp index a4fe17150cab0557919ac8211bb3c99bcc3c2dfa..9758d8f3d83db5ea6889d450f5371744c33c2fbe 100644 --- a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp +++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp @@ -17,7 +17,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -38,7 +38,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; IKRRTWindow::IKRRTWindow(std::string& sceneFile, std::string& reachFile, std::string& rns, std::string& eef, std::string& colModel, std::string& colModelRob) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -311,9 +311,9 @@ void IKRRTWindow::buildVisu() if (obstacles.size() > 0) { - for (size_t i = 0; i < obstacles.size(); i++) + for (const auto & obstacle : obstacles) { - SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(obstacles[i], colModel); + SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(obstacle, colModel); if (visualisationNode) { diff --git a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanning.cpp b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanning.cpp index 4450e8e2f3c7ffa973b5756d0ce663075bd06846..91d19587377786ff07fd734a212182db2be964c1 100644 --- a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanning.cpp +++ b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanning.cpp @@ -2,7 +2,7 @@ #include "MTPlanningWindow.h" -#include <string.h> +#include <cstring> #include <iostream> using namespace std; using namespace VirtualRobot; diff --git a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp index 6f8d18e7be1040c0040cede3b42d995a9944f70c..4a59357fec4cc464a8f05ad6f39c44b2023b4f97 100644 --- a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp +++ b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningScenery.cpp @@ -55,14 +55,14 @@ MTPlanningScenery::MTPlanningScenery() kinChainName = "All"; TCPName = "Visu"; - obstSep = NULL; + obstSep = nullptr; robotFilename = "robots/examples/MultiThreadedPlanning/CartMover.xml"; VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robotFilename); plannersStarted = false; optimizeStarted = false; - startEndVisu = NULL; + startEndVisu = nullptr; buildScene(); } @@ -77,9 +77,9 @@ MTPlanningScenery::~MTPlanningScenery() void MTPlanningScenery::reset() { - for (int i = 0; i < (int)robots.size(); i++) + for (auto & robot : robots) { - robots[i].reset(); + robot.reset(); } robots.clear(); @@ -94,30 +94,30 @@ void MTPlanningScenery::reset() stopOptimizing(); } - for (int i = 0; i < (int)planners.size(); i++) + for (auto & planner : planners) { - planners[i].reset(); + planner.reset(); } planners.clear(); - for (int i = 0; i < (int)CSpaces.size(); i++) + for (auto & CSpace : CSpaces) { - CSpaces[i].reset(); + CSpace.reset(); } CSpaces.clear(); - for (int i = 0; i < (int)planningThreads.size(); i++) + for (auto & planningThread : planningThreads) { - planningThreads[i].reset(); + planningThread.reset(); } planningThreads.clear(); - for (int i = 0; i < (int)optimizeThreads.size(); i++) + for (auto & optimizeThread : optimizeThreads) { - optimizeThreads[i].reset(); + optimizeThread.reset(); } optimizeThreads.clear(); @@ -125,22 +125,22 @@ void MTPlanningScenery::reset() solutions.clear(); optiSolutions.clear(); - for (int i = 0; i < (int)visualisations.size(); i++) + for (auto & visualisation : visualisations) { - if (visualisations[i] != NULL) + if (visualisation != nullptr) { - sceneSep->removeChild(visualisations[i]); + sceneSep->removeChild(visualisation); } } visualisations.clear(); - if (startEndVisu != NULL) + if (startEndVisu != nullptr) { sceneSep->removeChild(startEndVisu); } - startEndVisu = NULL; + startEndVisu = nullptr; ///////////////////////////////////////////////////////////////////////////// @@ -185,7 +185,7 @@ void MTPlanningScenery::buildScene() if (obstSep) { sceneSep->removeChild(obstSep); - obstSep = NULL; + obstSep = nullptr; } float fCubeSize = 50.0f; @@ -213,7 +213,7 @@ void MTPlanningScenery::buildScene() o->setGlobalPose(m); environment->addSceneObject(o); boost::shared_ptr<CoinVisualization> visualization = o->getVisualization<CoinVisualization>(); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -361,9 +361,9 @@ void MTPlanningScenery::buildPlanningThread(bool bMultiCollisionCheckers, int id solutions.push_back(CSpacePathPtr()); optiSolutions.push_back(CSpacePathPtr()); optimizeThreads.push_back(PathProcessingThreadPtr()); - visualisations.push_back(NULL); + visualisations.push_back(nullptr); - if (startEndVisu == NULL) + if (startEndVisu == nullptr) { startEndVisu = new SoSeparator(); sceneSep->addChild(startEndVisu); @@ -451,14 +451,14 @@ void MTPlanningScenery::stopPlanning() { cout << "Stopping " << planningThreads.size() << " planning threads..." << endl; - for (unsigned int i = 0; i < planningThreads.size(); i++) + for (auto & planningThread : planningThreads) { - planningThreads[i]->stop(); + planningThread->stop(); } - for (unsigned int i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - robots[i]->setUpdateVisualization(true); + robot->setUpdateVisualization(true); } cout << "... done" << endl; @@ -476,14 +476,14 @@ void MTPlanningScenery::stopOptimizing() cout << "Stopping " << optimizeThreads.size() << " optimizing threads..." << endl; - for (int i = 0; i < (int)optimizeThreads.size(); i++) + for (auto & optimizeThread : optimizeThreads) { - optimizeThreads[i]->stop(); + optimizeThread->stop(); } - for (int i = 0; i < (int)robots.size(); i++) + for (auto & robot : robots) { - robots[i]->setUpdateVisualization(true); + robot->setUpdateVisualization(true); } cout << "...done" << endl; @@ -501,14 +501,14 @@ void MTPlanningScenery::startPlanning() cout << "Starting " << planningThreads.size() << " planning threads..." << endl; - for (unsigned int i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - robots[i]->setUpdateVisualization(false); + robot->setUpdateVisualization(false); } - for (unsigned int i = 0; i < planningThreads.size(); i++) + for (auto & planningThread : planningThreads) { - planningThreads[i]->start(); + planningThread->start(); } cout << "... done" << endl; @@ -537,9 +537,9 @@ void MTPlanningScenery::startOptimizing() if (plannersStarted) { - for (int i = 0; i < (int)planningThreads.size(); i++) + for (auto & planningThread : planningThreads) { - if (planningThreads[i]->isRunning()) + if (planningThread->isRunning()) { cout << "Planning is not finish!..." << endl; return; @@ -564,16 +564,16 @@ void MTPlanningScenery::startOptimizing() int j = 0; - for (unsigned int i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - robots[i]->setUpdateVisualization(false); + robot->setUpdateVisualization(false); } - for (unsigned int i = 0; i < optimizeThreads.size(); i++) + for (auto & optimizeThread : optimizeThreads) { - if (optimizeThreads[i]) + if (optimizeThread) { - optimizeThreads[i]->start(SHORTEN_LOOP); + optimizeThread->start(SHORTEN_LOOP); j++; } } @@ -852,10 +852,8 @@ void MTPlanningScenery::getThreadCount(int& nWorking, int& nIdle) nWorking = 0; nIdle = 0; - for (unsigned int i = 0; i < planningThreads.size(); i++) + for (auto pThread : planningThreads) { - PlanningThreadPtr pThread = planningThreads[i]; - if (pThread->isRunning()) { nWorking++; @@ -873,10 +871,8 @@ void MTPlanningScenery::getOptimizeThreadCount(int& nWorking, int& nIdle) nWorking = 0; nIdle = 0; - for (unsigned int i = 0; i < optimizeThreads.size(); i++) + for (auto pOptiThread : optimizeThreads) { - PathProcessingThreadPtr pOptiThread = optimizeThreads[i]; - if (pOptiThread && pOptiThread->isRunning()) { nWorking++; diff --git a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.cpp b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.cpp index c4c3eda10d767b4c3d862dbdecfa28b601fd58ec..d2c6dfdd999549b9633c2791c04ec30a05f6ae87 100644 --- a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.cpp +++ b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.cpp @@ -31,14 +31,14 @@ bool runtimeDisplayed = false; bool optiTimeDisplayed = false; MTPlanningWindow::MTPlanningWindow() - : QMainWindow(NULL) + : QMainWindow(nullptr) { //resize(1100, 768); - graspObjectSep = NULL; - robotSep = NULL; + graspObjectSep = nullptr; + robotSep = nullptr; - scene = NULL; + scene = nullptr; scene = new MTPlanningScenery(); sceneSep = scene->getScene(); @@ -58,7 +58,7 @@ MTPlanningWindow::MTPlanningWindow() MTPlanningWindow::~MTPlanningWindow() { - if (scene != NULL) + if (scene != nullptr) { delete scene; } diff --git a/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp b/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp index da2c062dfb8d61560be68fd0b7e1f0a0c9c3c6c3..0b30535b93820de944e8ceb0b00b963cba831389 100644 --- a/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp +++ b/MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp @@ -19,7 +19,7 @@ #include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h> #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -42,7 +42,7 @@ PlatformWindow::PlatformWindow(const std::string& sceneFile, const std::string& rns, const std::string& colModelRob, const std::string& colModelEnv) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -146,7 +146,7 @@ void PlatformWindow::buildVisu() if (scene) { visualization = scene->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { diff --git a/MotionPlanning/examples/RRT/RRTdemo.cpp b/MotionPlanning/examples/RRT/RRTdemo.cpp index bbf178cef018a266a11f7f9c9f11a3d1731be562..f5b97682e927b218bfeb653bf25f385e0290ad53 100644 --- a/MotionPlanning/examples/RRT/RRTdemo.cpp +++ b/MotionPlanning/examples/RRT/RRTdemo.cpp @@ -30,7 +30,7 @@ QWidget* win; void show(SoNode* n) { - if (win == NULL) + if (win == nullptr) { printf("Could not create window.\n"); exit(-3); @@ -175,7 +175,7 @@ void startRRTVisualization() SceneObject::VisualizationType colModel = SceneObject::Full; boost::shared_ptr<CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp index e8e8b3dd4f96c5ffe90434010ad60bfe49c46167..1d6713fba61ed8a846ccdaf27bd16b5bb8009b2e 100644 --- a/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp +++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp @@ -17,7 +17,7 @@ #include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h> #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -37,7 +37,7 @@ float TIMER_MS = 200.0f; RrtGuiWindow::RrtGuiWindow(const std::string& sceneFile, const std::string& sConf, const std::string& gConf, const std::string& rns, const std::string& colModelRob1, const std::string& colModelRob2, const std::string& colModelEnv) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -188,7 +188,7 @@ void RrtGuiWindow::buildVisu() if (scene) { visualization = scene->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -294,9 +294,9 @@ void RrtGuiWindow::loadScene() UI.comboBoxGoal->clear(); UI.comboBoxStart->clear(); - for (size_t i = 0; i < configs.size(); i++) + for (auto & config : configs) { - QString qtext = configs[i]->getName().c_str(); + QString qtext = config->getName().c_str(); UI.comboBoxStart->addItem(qtext); UI.comboBoxGoal->addItem(qtext); } @@ -310,9 +310,9 @@ void RrtGuiWindow::loadScene() UI.comboBoxColModelEnv->clear(); QString qtext; - for (size_t i = 0; i < soss.size(); i++) + for (auto & sos : soss) { - qtext = soss[i]->getName().c_str(); + qtext = sos->getName().c_str(); UI.comboBoxColModelEnv->addItem(qtext); } @@ -324,9 +324,9 @@ void RrtGuiWindow::loadScene() UI.comboBoxColModelRobotStatic->clear(); UI.comboBoxRNS->clear(); - for (size_t i = 0; i < rnss.size(); i++) + for (auto & rns : rnss) { - qtext = rnss[i]->getName().c_str(); + qtext = rns->getName().c_str(); UI.comboBoxColModelRobot->addItem(qtext); UI.comboBoxColModelRobotStatic->addItem(qtext); UI.comboBoxRNS->addItem(qtext); diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp index 05748484c82d6d1e335c46398f524d857a976b7b..5346d5ffc2d6546b6d489e88b5393c6cfa5cdf7d 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletCoinQtViewer.cpp @@ -48,7 +48,7 @@ namespace SimDynamics //SoSelection *selection = new SoSelection(); //sceneGraph->addChild( selection ); - viewer = NULL; + viewer = nullptr; // register callback SoSensorManager* sensor_mgr = SoDB::getSensorManager(); @@ -65,7 +65,7 @@ namespace SimDynamics { stopCB(); sceneGraphRoot->unref(); - sceneGraphRoot = NULL; + sceneGraphRoot = nullptr; } void BulletCoinQtViewer::selectionCB(void* userdata, SoPath* path) @@ -414,7 +414,7 @@ namespace SimDynamics SoSensorManager* sensor_mgr = SoDB::getSensorManager(); sensor_mgr->removeTimerSensor(timerSensor); delete timerSensor; - timerSensor = NULL; + timerSensor = nullptr; } if (sceneGraph) diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp index b343fc4ee66a88267c75b0b5440c7f0aa96fe18c..7fa7948710311425a751154660c942539d60d0dd 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp @@ -33,11 +33,11 @@ namespace SimDynamics BulletEngine::BulletEngine(boost::shared_ptr <boost::recursive_mutex> engineMutex) : DynamicsEngine(engineMutex) { - collision_config = NULL; - dispatcher = NULL; - overlappingPairCache = NULL; - constraintSolver = NULL; - dynamicsWorld = NULL; + collision_config = nullptr; + dispatcher = nullptr; + overlappingPairCache = nullptr; + constraintSolver = nullptr; + dynamicsWorld = nullptr; simTime = 0; } @@ -141,17 +141,17 @@ namespace SimDynamics } delete dynamicsWorld; - dynamicsWorld = NULL; + dynamicsWorld = nullptr; delete collision_config; - collision_config = NULL; + collision_config = nullptr; delete dispatcher; - dispatcher = NULL; + dispatcher = nullptr; delete overlappingPairCache; - overlappingPairCache = NULL; + overlappingPairCache = nullptr; delete constraintSolver; - constraintSolver = NULL; + constraintSolver = nullptr; delete collisionFilterCallback; - collisionFilterCallback = NULL; + collisionFilterCallback = nullptr; return true; } @@ -256,7 +256,7 @@ namespace SimDynamics } dynamicsWorld->removeRigidBody(btObject->getRigidBody().get()); - btObject->getRigidBody()->setBroadphaseHandle(NULL); + btObject->getRigidBody()->setBroadphaseHandle(nullptr); return DynamicsEngine::removeObject(o); } @@ -429,14 +429,14 @@ namespace SimDynamics std::vector<BulletRobot::LinkInfo> links = btRobot->getLinks(); std::vector<DynamicsObjectPtr> nodes = btRobot->getDynamicsRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (const auto & node : nodes) { - addObject(nodes[i]); + addObject(node); } - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - addLink(links[i]); + addLink(link); } return DynamicsEngine::addRobot(r); @@ -463,19 +463,19 @@ namespace SimDynamics e->updateRobots(timeStep); - for (unsigned int i = 0; i < e->callbacks.size(); i++) + for (auto & callback : e->callbacks) { - e->callbacks[i].first(e->callbacks[i].second, timeStep); + callback.first(callback.second, timeStep); } } void BulletEngine::updateRobots(btScalar timeStep) { - for (size_t i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - robots[i]->actuateJoints(static_cast<double>(timeStep)); - robots[i]->updateSensors(static_cast<double>(timeStep)); + robot->actuateJoints(static_cast<double>(timeStep)); + robot->updateSensors(static_cast<double>(timeStep)); } } @@ -494,14 +494,14 @@ namespace SimDynamics std::vector<DynamicsObjectPtr> nodes = btRobot->getDynamicsRobotNodes(); - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - removeLink(links[i]); + removeLink(link); } - for (size_t i = 0; i < nodes.size(); i++) + for (const auto & node : nodes) { - removeObject(nodes[i]); + removeObject(node); } return DynamicsEngine::removeRobot(r); @@ -516,9 +516,9 @@ namespace SimDynamics dynamicsWorld->addConstraint(l.joint.get(), true); #endif - for (size_t i = 0; i < l.disabledCollisionPairs.size(); i++) + for (auto & disabledCollisionPair : l.disabledCollisionPairs) { - this->disableCollision(static_cast<DynamicsObject*>(l.disabledCollisionPairs[i].first.get()), static_cast<DynamicsObject*>(l.disabledCollisionPairs[i].second.get())); + this->disableCollision(static_cast<DynamicsObject*>(disabledCollisionPair.first.get()), static_cast<DynamicsObject*>(disabledCollisionPair.second.get())); } return true; @@ -714,9 +714,9 @@ namespace SimDynamics std::vector<BulletRobot::LinkInfo> links = br->getLinks(bo); - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - removeLink(links[i]); + removeLink(link); } bool res = br->detachObject(object); @@ -741,9 +741,9 @@ void SimDynamics::BulletEngine::updateAction(btCollisionWorld *collisionWorld, b updateRobots(deltaTimeStep); - for (unsigned int i = 0; i < callbacks.size(); i++) + for (auto & callback : callbacks) { - callbacks[i].first(callbacks[i].second, deltaTimeStep); + callback.first(callback.second, deltaTimeStep); } std::chrono::duration<double> diff = (std::chrono::system_clock::now()-start); // cout << "duration: " << diff.count() << endl; diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp index 0946703a7a3f3179ad74519390189b2d994efc7e..2984161d0fb577131c8cd5dd987776825ac5c319 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp @@ -7,13 +7,11 @@ namespace SimDynamics { BulletEngineFactory::BulletEngineFactory() - { - } + = default; BulletEngineFactory::~BulletEngineFactory() - { - } + = default; /** diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/DemoApplication.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/DemoApplication.cpp index 8aee95256ab755d69a24f33b7e4a99f68f658999..cc9f63da38f6fb92316df07fed97d7b399eeacfd 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/DemoApplication.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/DemoApplication.cpp @@ -58,9 +58,9 @@ extern int gTotalBytesAlignedAllocs; DemoApplication::DemoApplication() //see btIDebugDraw.h for modes : - m_dynamicsWorld(0), - m_pickConstraint(0), - m_shootBoxShape(0), + m_dynamicsWorld(nullptr), + m_pickConstraint(nullptr), + m_shootBoxShape(nullptr), m_cameraDistance(15.0), m_debugMode(0), m_ele(20.f), @@ -126,7 +126,7 @@ void DemoApplication::overrideGLShapeDrawer(GL_ShapeDrawer* shapeDrawer) m_shapeDrawer = shapeDrawer; } -void DemoApplication::myinit(void) +void DemoApplication::myinit() { GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) }; @@ -772,7 +772,7 @@ int gPickingConstraintId = 0; btVector3 gOldPickingPos; btVector3 gHitPos(-1, -1, -1); btScalar gOldPickingDist = 0.f; -btRigidBody* pickedBody = 0;//for deactivation state +btRigidBody* pickedBody = nullptr;//for deactivation state btVector3 DemoApplication::getRayTo(int x, int y) @@ -1070,10 +1070,10 @@ void DemoApplication::removePickingConstraint() m_dynamicsWorld->removeConstraint(m_pickConstraint); delete m_pickConstraint; //printf("removed constraint %i",gPickingConstraintId); - m_pickConstraint = 0; + m_pickConstraint = nullptr; pickedBody->forceActivationState(ACTIVE_TAG); pickedBody->setDeactivationTime(0.f); - pickedBody = 0; + pickedBody = nullptr; } } diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.cpp index 4dd9c2779d75c850ef28d286f57bb9330f3de620..2a3375169dbe3277c07077d25ac185ec56bcca23 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.cpp @@ -5,7 +5,7 @@ -#include <stdio.h> //printf debugging +#include <cstdio> //printf debugging GLDebugDrawer::GLDebugDrawer() : m_debugMode(0) { @@ -13,8 +13,7 @@ GLDebugDrawer::GLDebugDrawer() } GLDebugDrawer::~GLDebugDrawer() -{ -} += default; void GLDebugDrawer::drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor) { diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugFont.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugFont.cpp index 90540a8b378409165902335e2e4979151b99be44..c6d3d467cc4826c36d039252767e5a58b8dfc1cb 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugFont.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugFont.cpp @@ -45,8 +45,8 @@ subject to the following restrictions: #endif #endif -#include <stdio.h> -#include <string.h> //for memset +#include <cstdio> +#include <cstring> //for memset extern unsigned char sFontData[]; static bool sTexturesInitialized = false; diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogDynamicsWorld.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogDynamicsWorld.cpp index 365653bb5f81a980b276a6a56bd9fb4a0f9141ef..ccd7cfbd16991b2cbf2ddb826b5ddee6487c87a6 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogDynamicsWorld.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogDynamicsWorld.cpp @@ -25,10 +25,10 @@ subject to the following restrictions: GL_DialogDynamicsWorld::GL_DialogDynamicsWorld() { - m_upperBorder = 0; - m_lowerBorder = 0; + m_upperBorder = nullptr; + m_lowerBorder = nullptr; - m_pickConstraint = 0; + m_pickConstraint = nullptr; m_screenWidth = 0; m_screenHeight = 0; @@ -196,7 +196,7 @@ GL_DialogWindow* GL_DialogDynamicsWorld::createDialog(int horPos, int vertPos btScalar mass = 100.f; btVector3 localInertia; boxShape->calculateLocalInertia(mass, localInertia); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, 0, boxShape, localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, nullptr, boxShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); btTransform trans; trans.setIdentity(); @@ -227,7 +227,7 @@ GL_SliderControl* GL_DialogDynamicsWorld::createSlider(GL_DialogWindow* dialog, btScalar mass = .1f; btVector3 localInertia; boxShape->calculateLocalInertia(mass, localInertia); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, 0, boxShape, localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, nullptr, boxShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); btTransform trans; trans.setIdentity(); @@ -306,7 +306,7 @@ GL_ToggleControl* GL_DialogDynamicsWorld::createToggle(GL_DialogWindow* dialog, btScalar mass = 0.1f; btVector3 localInertia; boxShape->calculateLocalInertia(mass, localInertia); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, 0, boxShape, localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, nullptr, boxShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); btTransform trans; trans.setIdentity(); @@ -366,7 +366,7 @@ void GL_DialogDynamicsWorld::draw(btScalar timeStep) } } -static btRigidBody* pickedBody = 0;//for deactivation state +static btRigidBody* pickedBody = nullptr;//for deactivation state static btScalar mousePickClamping = 111130.f; //static int gPickingConstraintId = 0; @@ -455,7 +455,7 @@ bool GL_DialogDynamicsWorld::mouseFunc(int button, int state, int x, int y) btVector3 rayFrom; - if (1)//m_ortho) + if (true)//m_ortho) { rayFrom = rayTo; rayFrom.setZ(-100.f); @@ -565,7 +565,7 @@ bool GL_DialogDynamicsWorld::mouseFunc(int button, int state, int x, int y) m_dynamicsWorld->removeConstraint(m_pickConstraint); delete m_pickConstraint; //printf("removed constraint %i",gPickingConstraintId); - m_pickConstraint = 0; + m_pickConstraint = nullptr; pickedBody->forceActivationState(ACTIVE_TAG); pickedBody->setDeactivationTime(0.f); @@ -579,7 +579,7 @@ bool GL_DialogDynamicsWorld::mouseFunc(int button, int state, int x, int y) { GL_SliderControl* sliderControl = (GL_SliderControl*) ctrl; - btSliderConstraint* slider = 0; + btSliderConstraint* slider = nullptr; btTypedConstraint* constraint = sliderControl->getConstraint(); if (constraint->getConstraintType() == SLIDER_CONSTRAINT_TYPE) @@ -614,7 +614,7 @@ bool GL_DialogDynamicsWorld::mouseFunc(int button, int state, int x, int y) } - pickedBody = 0; + pickedBody = nullptr; } @@ -643,7 +643,7 @@ btVector3 GL_DialogDynamicsWorld::getRayTo(int x, int y) btVector3 cameraTargetPosition(0, 0, 0); btVector3 cameraUp(0, -1, 0); - if (1)//_ortho) + if (true)//_ortho) { btScalar aspect; @@ -754,7 +754,7 @@ void GL_DialogDynamicsWorld::mouseMotionFunc(int x, int y) btVector3 oldPivotInB = p2p->getPivotInB(); btVector3 newPivotB; - if (1)//_ortho) + if (true)//_ortho) { newPivotB = oldPivotInB; newPivotB.setX(newRayTo.getX()); diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp index c4919bd426c929dbd801c20eaccfc4941343999d..1c5d188f7b4f3308b71ec0bef3af736d84d8c9c0 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "GLDebugFont.h" #include "btBulletDynamicsCommon.h" -#include <stdio.h> // for sprintf() +#include <cstdio> // for sprintf() #define USE_ARRAYS 1 @@ -45,9 +45,7 @@ void GL_DialogWindow::setScreenSize(int width, int height) m_screenHeight = height; } GL_DialogWindow::~GL_DialogWindow() -{ - -} += default; diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp index 8927289ccecffbaf0af794512f69321e20f4672d..c8d55bce48264eea8259b9e900cd3f7895d83cca 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp @@ -46,7 +46,7 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" //for debugmodes -#include <stdio.h> //printf debugging +#include <cstdio> //printf debugging //#define USE_DISPLAY_LISTS 1 #ifdef USE_DISPLAY_LISTS @@ -776,7 +776,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons { if (shape->isConvex()) { - const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : 0; + const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : nullptr; if (poly) { diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.cpp index 75a00b66da2303e2848be5a4cf93f1fd0e99a3ab..18d8a9f148d52ee1ee56359d380ef803189b0058 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.cpp @@ -30,13 +30,12 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" GL_Simplex1to4::GL_Simplex1to4() - : m_simplexSolver(0) + : m_simplexSolver(nullptr) { } GL_Simplex1to4::~GL_Simplex1to4() -{ -} += default; /// /// Debugging method calcClosest calculates the closest point to the origin, using m_simplexSolver diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutStuff.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutStuff.cpp index a37566a37caf31cdef6d5bb61bfa8fdf5285352c..bf74c7a8fa7cb63abee7a075c81ebeab99d8df0f 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutStuff.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutStuff.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: #include "DemoApplication.h" //glut is C code, this global gDemoApplication links glut to the C++ demo -static DemoApplication* gDemoApplication = 0; +static DemoApplication* gDemoApplication = nullptr; #include "GlutStuff.h" @@ -66,7 +66,7 @@ static void glutMotionFuncCallback(int x, int y) } -static void glutDisplayCallback(void) +static void glutDisplayCallback() { gDemoApplication->displayCallback(); } diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/stb_image.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/stb_image.cpp index 263750e6bde4b8f4f5e2a363fbc14e73bfe2627a..7a0c7d067fa8cf4f8f331b140cb46e2bddf813e8 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/stb_image.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/stb_image.cpp @@ -3,17 +3,17 @@ #ifndef STBI_HEADER_FILE_ONLY #ifndef STBI_NO_HDR -#include <math.h> // ldexp -#include <string.h> // strcmp, strtok +#include <cmath> // ldexp +#include <cstring> // strcmp, strtok #endif #ifndef STBI_NO_STDIO -#include <stdio.h> +#include <cstdio> #endif -#include <stdlib.h> +#include <cstdlib> #include <memory.h> -#include <assert.h> -#include <stdarg.h> +#include <cassert> +#include <cstdarg> #ifndef _MSC_VER #ifdef __cplusplus @@ -27,12 +27,12 @@ // implementation: -typedef unsigned char uint8; -typedef unsigned short uint16; -typedef signed short int16; -typedef unsigned int uint32; -typedef signed int int32; -typedef unsigned int uint; +using uint8 = unsigned char; +using uint16 = unsigned short; +using int16 = short; +using uint32 = unsigned int; +using int32 = int; +using uint = unsigned int; // should produce compiler error if size is wrong typedef unsigned char validate_uint32[sizeof(uint32) == 4 ? 1 : -1]; @@ -81,7 +81,7 @@ static void refill_buffer(stbi* s); // initialize a memory-decode context static void start_mem(stbi* s, uint8 const* buffer, int len) { - s->io.read = NULL; + s->io.read = nullptr; s->read_from_callbacks = 0; s->img_buffer = s->img_buffer_original = (uint8*) buffer; s->img_buffer_end = (uint8*) buffer + len; @@ -604,7 +604,7 @@ assert(req_comp >= 1 && req_comp <= 4); good = (unsigned char*) malloc(req_comp * x * y); -if (good == NULL) +if (good == nullptr) { free(data); return epuc("outofmem", "Out of memory"); @@ -664,7 +664,7 @@ static float* ldr_to_hdr(stbi_uc* data, int x, int y, int comp) int i, k, n; float* output = (float*) malloc(x * y * comp * sizeof(float)); -if (output == NULL) +if (output == nullptr) { free(data); return epf("outofmem", "Out of memory"); @@ -703,7 +703,7 @@ static stbi_uc* hdr_to_ldr(float* data, int x, int y, int comp) int i, k, n; stbi_uc* output = (stbi_uc*) malloc(x * y * comp); -if (output == NULL) +if (output == nullptr) { free(data); return epuc("outofmem", "Out of memory"); @@ -1171,7 +1171,7 @@ return (uint8) x; #ifdef STBI_SIMD typedef unsigned short stbi_dequantize_t; #else -typedef uint8 stbi_dequantize_t; +using stbi_dequantize_t = uint8; #endif // .344 seconds on 3*anemones.jpg @@ -1647,8 +1647,8 @@ s->img_n = c; for (i = 0; i < c; ++i) { - z->img_comp[i].data = NULL; - z->img_comp[i].linebuf = NULL; + z->img_comp[i].data = nullptr; + z->img_comp[i].linebuf = nullptr; } if (Lf != 8 + 3 * s->img_n) @@ -1733,12 +1733,12 @@ for (i = 0; i < s->img_n; ++i) z->img_comp[i].h2 = z->img_mcu_y * z->img_comp[i].v * 8; z->img_comp[i].raw_data = malloc(z->img_comp[i].w2 * z->img_comp[i].h2 + 15); - if (z->img_comp[i].raw_data == NULL) + if (z->img_comp[i].raw_data == nullptr) { for (--i; i >= 0; --i) { free(z->img_comp[i].raw_data); - z->img_comp[i].data = NULL; + z->img_comp[i].data = nullptr; } return e("outofmem", "Out of memory"); @@ -1746,7 +1746,7 @@ for (i = 0; i < s->img_n; ++i) // align blocks for installable-idct using mmx/sse z->img_comp[i].data = (uint8*)(((size_t) z->img_comp[i].raw_data + 15) & ~15); - z->img_comp[i].linebuf = NULL; + z->img_comp[i].linebuf = nullptr; } return 1; @@ -1869,8 +1869,7 @@ return 1; // static jfif-centered resampling (across block boundaries) -typedef uint8* (*resample_row_func)(uint8* out, uint8* in0, uint8* in1, - int w, int hs); +using resample_row_func = uint8 *(*)(uint8 *, uint8 *, uint8 *, int, int); #define div4(x) ((uint8) ((x) >> 2)) @@ -2059,13 +2058,13 @@ for (i = 0; i < j->s->img_n; ++i) if (j->img_comp[i].data) { free(j->img_comp[i].raw_data); - j->img_comp[i].data = NULL; + j->img_comp[i].data = nullptr; } if (j->img_comp[i].linebuf) { free(j->img_comp[i].linebuf); - j->img_comp[i].linebuf = NULL; + j->img_comp[i].linebuf = nullptr; } } } @@ -2096,7 +2095,7 @@ z->s->img_n = 0; if (!decode_jpeg_image(z)) { cleanup_jpeg(z); - return NULL; + return nullptr; } // determine actual number of components to generate @@ -2527,7 +2526,7 @@ while (cur + n > limit) q = (char*) realloc(z->zout_start, limit); -if (q == NULL) +if (q == nullptr) { return e("outofmem", "Out of memory"); } @@ -2783,7 +2782,7 @@ return 1; // @TODO: should statically initialize these for optimal thread safety static uint8 default_length[288], default_distance[32]; -static void init_defaults(void) +static void init_defaults() { int i; // use <= to match clearly with spec @@ -2902,9 +2901,9 @@ char* stbi_zlib_decode_malloc_guesssize(const char* buffer, int len, int initial zbuf a; char* p = (char*) malloc(initial_size); -if (p == NULL) +if (p == nullptr) { - return NULL; + return nullptr; } a.zbuffer = (uint8*) buffer; @@ -2922,7 +2921,7 @@ if (do_zlib(&a, p, initial_size, 1, 1)) else { free(a.zout_start); - return NULL; + return nullptr; } } @@ -2936,9 +2935,9 @@ char* stbi_zlib_decode_malloc_guesssize_headerflag(const char* buffer, int len, zbuf a; char* p = (char*) malloc(initial_size); -if (p == NULL) +if (p == nullptr) { - return NULL; + return nullptr; } a.zbuffer = (uint8*) buffer; @@ -2956,7 +2955,7 @@ if (do_zlib(&a, p, initial_size, 1, parse_header)) else { free(a.zout_start); - return NULL; + return nullptr; } } @@ -2981,9 +2980,9 @@ char* stbi_zlib_decode_noheader_malloc(char const* buffer, int len, int* outlen) zbuf a; char* p = (char*) malloc(16384); -if (p == NULL) +if (p == nullptr) { - return NULL; + return nullptr; } a.zbuffer = (uint8*) buffer; @@ -3001,7 +3000,7 @@ if (do_zlib(&a, p, 16384, 1, 0)) else { free(a.zout_start); - return NULL; + return nullptr; } } @@ -3355,7 +3354,7 @@ uint8* p, *temp_out, *orig = a->out; p = (uint8*) malloc(pixel_count * pal_img_n); -if (p == NULL) +if (p == nullptr) { return e("outofmem", "Out of memory"); } @@ -3472,9 +3471,9 @@ uint32 ioff = 0, idata_limit = 0, i, pal_len = 0; int first = 1, k, interlace = 0, iphone = 0; stbi* s = z->s; -z->expanded = NULL; -z->idata = NULL; -z->out = NULL; +z->expanded = nullptr; +z->idata = nullptr; +z->out = nullptr; if (!check_png_header(s)) { @@ -3731,7 +3730,7 @@ for (;;) p = (uint8*) realloc(z->idata, idata_limit); - if (p == NULL) + if (p == nullptr) { return e("outofmem", "Out of memory"); } @@ -3762,20 +3761,20 @@ for (;;) return 1; } - if (z->idata == NULL) + if (z->idata == nullptr) { return e("no IDAT", "Corrupt PNG"); } z->expanded = (uint8*) stbi_zlib_decode_malloc_guesssize_headerflag((char*) z->idata, ioff, 16384, (int*) &raw_len, !iphone); - if (z->expanded == NULL) + if (z->expanded == nullptr) { return 0; // zlib should set error } free(z->idata); - z->idata = NULL; + z->idata = nullptr; if ((req_comp == s->img_n + 1 && req_comp != 3 && !pal_img_n) || has_trans) { @@ -3820,7 +3819,7 @@ for (;;) } free(z->expanded); - z->expanded = NULL; + z->expanded = nullptr; return 1; } @@ -3856,7 +3855,7 @@ for (;;) static unsigned char* do_png(png* p, int* x, int* y, int* n, int req_comp) { -unsigned char* result = NULL; +unsigned char* result = nullptr; if (req_comp < 0 || req_comp > 4) { @@ -3866,14 +3865,14 @@ if (req_comp < 0 || req_comp > 4) if (parse_png_file(p, SCAN_load, req_comp)) { result = p->out; - p->out = NULL; + p->out = nullptr; if (req_comp && req_comp != p->s->img_out_n) { result = convert_format(result, p->s->img_out_n, req_comp, p->s->img_x, p->s->img_y); p->s->img_out_n = req_comp; - if (result == NULL) + if (result == nullptr) { return result; } @@ -3889,11 +3888,11 @@ if (parse_png_file(p, SCAN_load, req_comp)) } free(p->out); -p->out = NULL; +p->out = nullptr; free(p->expanded); -p->expanded = NULL; +p->expanded = nullptr; free(p->idata); -p->idata = NULL; +p->idata = nullptr; return result; } @@ -4416,7 +4415,7 @@ if (req_comp && req_comp != target) { out = convert_format(out, target, req_comp, s->img_x, s->img_y); - if (out == NULL) + if (out == nullptr) { return out; // convert_format frees input on failure } @@ -4583,7 +4582,7 @@ int tga_bits_per_pixel = get8u(s); int tga_inverted = get8u(s); // image data unsigned char* tga_data; -unsigned char* tga_palette = NULL; +unsigned char* tga_palette = nullptr; int i, j; unsigned char raw_data[4]; unsigned char trans_data[4]; @@ -4609,7 +4608,7 @@ if ( //(tga_indexed) || (tga_bits_per_pixel != 24) && (tga_bits_per_pixel != 32)) ) { - return NULL; // we don't report this as a bad TGA because we don't even know if it's TGA + return nullptr; // we don't report this as a bad TGA because we don't even know if it's TGA } // If I'm paletted, then I'll use the number of bits from the palette @@ -4816,7 +4815,7 @@ if (tga_inverted) } // clear my palette, if I had one -if (tga_palette != NULL) +if (tga_palette != nullptr) { free(tga_palette); } @@ -5059,7 +5058,7 @@ if (req_comp && req_comp != 4) { out = convert_format(out, 4, req_comp, w, h); - if (out == NULL) + if (out == nullptr) { return out; // convert_format frees input on failure } @@ -5219,7 +5218,7 @@ for (y = 0; y < height; ++y) for (x = 0; x < width; ++x, dest += 4) if (!pic_readval(s, packet->channel, dest)) { - return 0; + return nullptr; } break; @@ -5247,7 +5246,7 @@ for (y = 0; y < height; ++y) if (!pic_readval(s, packet->channel, value)) { - return 0; + return nullptr; } for (i = 0; i < count; ++i, dest += 4) @@ -5294,7 +5293,7 @@ for (y = 0; y < height; ++y) if (!pic_readval(s, packet->channel, value)) { - return 0; + return nullptr; } for (i = 0; i < count; ++i, dest += 4) @@ -5314,7 +5313,7 @@ for (y = 0; y < height; ++y) for (i = 0; i < count; ++i, dest += 4) if (!pic_readval(s, packet->channel, dest)) { - return 0; + return nullptr; } } @@ -5364,7 +5363,7 @@ memset(result, 0xff, x * y * 4); if (!pic_load2(s, x, y, comp, result)) { free(result); - result = 0; + result = nullptr; } *px = x; @@ -5491,7 +5490,7 @@ g->bgindex = get8(s); g->ratio = get8(s); g->transparent = -1; -if (comp != 0) +if (comp != nullptr) { *comp = 4; // can't actually tell whether it's 3 or 4 until we parse the comments } @@ -5711,18 +5710,18 @@ for (i = 0; i < g->w * g->h * 4; i += 4) static uint8* stbi_gif_load_next(stbi* s, stbi_gif* g, int* comp, int req_comp) { int i; -uint8* old_out = 0; +uint8* old_out = nullptr; -if (g->out == 0) +if (g->out == nullptr) { if (!stbi_gif_header(s, g, comp, 0)) { - return 0; // failure_reason set by stbi_gif_header + return nullptr; // failure_reason set by stbi_gif_header } g->out = (uint8*) malloc(4 * g->w * g->h); - if (g->out == 0) + if (g->out == nullptr) { return epuc("outofmem", "Out of memory"); } @@ -5737,7 +5736,7 @@ else old_out = g->out; g->out = (uint8*) malloc(4 * g->w * g->h); - if (g->out == 0) + if (g->out == nullptr) { return epuc("outofmem", "Out of memory"); } @@ -5812,9 +5811,9 @@ for (;;) o = stbi_process_gif_raster(s, g); - if (o == NULL) + if (o == nullptr) { - return NULL; + return nullptr; } if (req_comp && req_comp != 4) @@ -5865,14 +5864,14 @@ for (;;) static stbi_uc* stbi_gif_load(stbi* s, int* x, int* y, int* comp, int req_comp) { -uint8* u = 0; +uint8* u = nullptr; stbi_gif g = {0}; u = stbi_gif_load_next(s, &g, comp, req_comp); if (u == (void*) 1) { - u = 0; // end of animated gif marker + u = nullptr; // end of animated gif marker } if (u) @@ -6056,7 +6055,7 @@ if (strncmp(token, "+X ", 3)) } token += 3; -width = strtol(token, NULL, 10); +width = strtol(token, nullptr, 10); *x = width; *y = height; @@ -6090,7 +6089,7 @@ main_decode_loop: else { // Read RLE-encoded data - scanline = NULL; + scanline = nullptr; for (j = 0; j < height; ++j) { @@ -6124,7 +6123,7 @@ else return epf("invalid decoded scanline length", "corrupt HDR"); } - if (scanline == NULL) + if (scanline == nullptr) { scanline = (stbi_uc*) malloc(width * 4); } @@ -6232,7 +6231,7 @@ if (strncmp(token, "+X ", 3)) } token += 3; -*x = strtol(token, NULL, 10); +*x = strtol(token, nullptr, 10); *comp = 3; return 1; } diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp index 40e081fe0873f2eb450e2801731fe6d7dfb46866..7c6ce736290aed31ec2915a55ba248d85c5c6419 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.cpp @@ -91,9 +91,7 @@ namespace SimDynamics } BulletOpenGLViewer::~BulletOpenGLViewer() - { - - } + = default; void BulletOpenGLViewer::initPhysics() { @@ -101,7 +99,7 @@ namespace SimDynamics } - void BulletOpenGLViewer::myinit(void) + void BulletOpenGLViewer::myinit() { GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) }; diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp index 0e39b0129367495ce8f9a6c879ce1f8b7f0b6a3c..1e65be5bd56b6947861d98f2ff2af39e6d751526 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp @@ -58,8 +58,7 @@ namespace SimDynamics } BulletRobot::~BulletRobot() - { - } + = default; @@ -75,10 +74,9 @@ namespace SimDynamics robotNodes = robot->getRobotNodes(); - for (size_t i = 0; i < robotNodes.size(); i++) + for (auto rn : robotNodes) { - RobotNodePtr rn = robotNodes[i]; CollisionModelPtr colModel = rn->getCollisionModel(); if (colModel) @@ -174,13 +172,13 @@ namespace SimDynamics BulletObjectPtr drn1 = boost::dynamic_pointer_cast<BulletObject>(dynamicRobotNodes[rn]); VR_ASSERT(drn1); - for (size_t i = 0; i < ic.size(); i++) + for (const auto & i : ic) { - RobotNodePtr rn2 = robot->getRobotNode(ic[i]); + RobotNodePtr rn2 = robot->getRobotNode(i); if (!rn2) { - VR_ERROR << "Error while processing robot node <" << rn->getName() << ">: Ignored collision model <" << ic[i] << "> is not part of robot..." << endl; + VR_ERROR << "Error while processing robot node <" << rn->getName() << ">: Ignored collision model <" << i << "> is not part of robot..." << endl; } else { @@ -456,9 +454,9 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if (links[i].nodeA == node1 && links[i].nodeB == node2) + if (link.nodeA == node1 && link.nodeB == node2) { return true; } @@ -668,9 +666,9 @@ namespace SimDynamics boost::unordered_set<std::string> contactObjectNames; // this seems stupid and it is, but that is abstract interfaces for you. - for (std::vector<SensorPtr>::iterator it = sensors.begin(); it != sensors.end(); it++) + for (auto & sensor : sensors) { - ContactSensorPtr contactSensor = boost::dynamic_pointer_cast<ContactSensor>(*it); + ContactSensorPtr contactSensor = boost::dynamic_pointer_cast<ContactSensor>(sensor); if (contactSensor) { @@ -682,11 +680,8 @@ namespace SimDynamics std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts = world->getEngine()->getContacts(); boost::unordered_map<std::string, VirtualRobot::ContactSensor::ContactFrame> frameMap; - for (std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo>::iterator it = contacts.begin(); - it != contacts.end(); it++) + for (auto & contact : contacts) { - const SimDynamics::DynamicsEngine::DynamicsContactInfo& contact = *it; - if (contact.objectAName.empty() || contact.objectBName.empty()) { continue; @@ -723,9 +718,9 @@ namespace SimDynamics } // Update forces and torques - for (std::vector<SensorPtr>::iterator it = sensors.begin(); it != sensors.end(); it++) + for (auto & sensor : sensors) { - ForceTorqueSensorPtr ftSensor = boost::dynamic_pointer_cast<ForceTorqueSensor>(*it); + ForceTorqueSensorPtr ftSensor = boost::dynamic_pointer_cast<ForceTorqueSensor>(sensor); if (ftSensor) { @@ -741,7 +736,7 @@ namespace SimDynamics } else { - ContactSensorPtr contactSensor = boost::dynamic_pointer_cast<ContactSensor>(*it); + ContactSensorPtr contactSensor = boost::dynamic_pointer_cast<ContactSensor>(sensor); if (contactSensor) { @@ -758,11 +753,11 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if (links[i].nodeJoint == node /*|| links[i].nodeJoint2 == node*/) + if (link.nodeJoint == node /*|| links[i].nodeJoint2 == node*/) { - return links[i]; + return link; } } @@ -774,11 +769,11 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if ((links[i].dynNode1 == object1 && links[i].dynNode2 == object2) || (links[i].dynNode1 == object2 && links[i].dynNode2 == object1)) + if ((link.dynNode1 == object1 && link.dynNode2 == object2) || (link.dynNode1 == object2 && link.dynNode2 == object1)) { - return links[i]; + return link; } } @@ -791,11 +786,11 @@ namespace SimDynamics MutexLockPtr lock = getScopedLock(); std::vector<BulletRobot::LinkInfo> result; - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if (links[i].nodeJoint == node /*|| links[i].nodeJoint2 == node*/ || links[i].nodeA == node || links[i].nodeB == node) + if (link.nodeJoint == node /*|| links[i].nodeJoint2 == node*/ || link.nodeA == node || link.nodeB == node) { - result.push_back(links[i]); + result.push_back(link); } } @@ -807,11 +802,11 @@ namespace SimDynamics MutexLockPtr lock = getScopedLock(); std::vector<BulletRobot::LinkInfo> result; - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if (links[i].dynNode1 == node || links[i].dynNode2 == node) + if (link.dynNode1 == node || link.dynNode2 == node) { - result.push_back(links[i]); + result.push_back(link); } } @@ -964,9 +959,9 @@ namespace SimDynamics return true; // not a failure, object is not attached } - for (size_t i = 0; i < ls.size(); i++) + for (const auto & l : ls) { - res = res & removeLink(ls[i]); + res = res & removeLink(l); } VR_INFO << "Detached object " << object->getName() << " from robot " << robot->getName() << endl; @@ -977,9 +972,9 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if (links[i].nodeJoint == node /*|| links[i].nodeJoint2 == node*/) + if (link.nodeJoint == node /*|| links[i].nodeJoint2 == node*/) { return true; } diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp index 2a8019e06a3173cbe8d90992554b4dbd7ab0dc31..d18fbf9ef4797b11077f585924f20fe6856c22ec 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.cpp @@ -15,9 +15,7 @@ namespace SimDynamics } SimoxCollisionDispatcher::~SimoxCollisionDispatcher() - { - - } + = default; bool SimoxCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) { SimDynamics::BulletObject* o0 = static_cast<SimDynamics::BulletObject*>(body0->getUserPointer()); diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp index cc1e72c6c44fc85e06b69419e0a87122aeb2ece7..ce9f4a0bc3c0cd470bb257729dac2a8207e7ee41 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp @@ -49,9 +49,7 @@ namespace SimDynamics } SimoxMotionState::~SimoxMotionState() - { - - } + = default; void SimoxMotionState::setWorldTransform(const btTransform& worldPose) { @@ -138,11 +136,11 @@ namespace SimDynamics std::vector<BulletRobot::LinkInfo> links = bdr->getLinks(rn); // update all involved joint values - for (size_t i = 0; i < links.size(); i++) + for (auto & link : links) { - if (links[i].nodeJoint) + if (link.nodeJoint) { - float ja = float(bdr->getJointAngle(links[i].nodeJoint)); + float ja = float(bdr->getJointAngle(link.nodeJoint)); #ifdef _DEBUG if (boost::math::isnan(ja)) @@ -152,7 +150,7 @@ namespace SimDynamics #endif // we can update the joint value via an RobotNodeActuator - RobotNodeActuatorPtr rna(new RobotNodeActuator(links[i].nodeJoint)); + RobotNodeActuatorPtr rna(new RobotNodeActuator(link.nodeJoint)); rna->updateJointAngle(ja); } } diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp index 917721c3c371c86a1d5a0da88ef08e05108f564c..9318f44d16a2ca8e5a698c3afd6885e55cfb2375 100644 --- a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp +++ b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp @@ -88,9 +88,9 @@ namespace SimDynamics if (find(robots.begin(), robots.end(), r) == robots.end()) { - for (size_t i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - if (robots[i]->getRobot() == r->getRobot()) + if (robot->getRobot() == r->getRobot()) { VR_ERROR << "Only one DynamicsWrapper per robot allowed. Robot " << r->getName() << endl; return false; @@ -332,11 +332,11 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < objects.size(); i++) + for (auto & object : objects) { - if (objects[i]->getName() == objectName) + if (object->getName() == objectName) { - return objects[i]; + return object; } } @@ -347,9 +347,9 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < objects.size(); i++) + for (auto & object : objects) { - objects[i]->activate(); + object->activate(); } } @@ -364,11 +364,11 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - if (robots[i]->getRobot() == r) + if (robot->getRobot() == r) { - return robots[i]; + return robot; } } @@ -379,11 +379,11 @@ namespace SimDynamics { MutexLockPtr lock = getScopedLock(); - for (size_t i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - if (robots[i]->getName() == robName) + if (robot->getName() == robName) { - return robots[i]; + return robot; } } diff --git a/SimDynamics/DynamicsEngine/DynamicsObject.cpp b/SimDynamics/DynamicsEngine/DynamicsObject.cpp index c7be1b796e81325050c71bf7fb353b8c631c704b..d0b0b1fdb5d560e38c994a6f15f42d335ef2c9e8 100644 --- a/SimDynamics/DynamicsEngine/DynamicsObject.cpp +++ b/SimDynamics/DynamicsEngine/DynamicsObject.cpp @@ -13,8 +13,7 @@ namespace SimDynamics } DynamicsObject::~DynamicsObject() - { - } + = default; std::string DynamicsObject::getName() const { diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp index 1558a3b90cb189e3174468e7d9e280557b3bd439..ad3ac8b517fedf0fe75eaecb3a25e4e7c7493f2d 100644 --- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp +++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp @@ -15,8 +15,7 @@ namespace SimDynamics } DynamicsRobot::~DynamicsRobot() - { - } + = default; std::string DynamicsRobot::getName() const { @@ -454,12 +453,12 @@ namespace SimDynamics void DynamicsRobot::enableSelfCollisions(bool enable) { // deactivate all self collisions - for (size_t i = 0; i < robotNodes.size(); i++) + for (const auto & i : robotNodes) { - auto drn1 = getDynamicsRobotNode(robotNodes.at(i)); - for (size_t j = 0; j < robotNodes.size(); j++) + auto drn1 = getDynamicsRobotNode(i); + for (const auto & robotNode : robotNodes) { - auto drn2 = getDynamicsRobotNode(robotNodes.at(j)); + auto drn2 = getDynamicsRobotNode(robotNode); if(enable) DynamicsWorld::GetWorld()->getEngine()->enableCollision(drn1.get(), drn2.get()); else diff --git a/SimDynamics/DynamicsWorld.cpp b/SimDynamics/DynamicsWorld.cpp index ee67afd882860a72c1c94fca73b90aade02c20bf..9d4f99b603de0e0ce92f2f549c3b70571c75531d 100644 --- a/SimDynamics/DynamicsWorld.cpp +++ b/SimDynamics/DynamicsWorld.cpp @@ -81,8 +81,7 @@ namespace SimDynamics } DynamicsWorld::~DynamicsWorld() - { - } + = default; bool DynamicsWorld::addObject(DynamicsObjectPtr o) { diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp index a79e305f98091a8e647c224c4685ecd5087c8f03..dc8ad157274b15d993ffb1e834288ddb5bc78a8a 100644 --- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp +++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp @@ -9,7 +9,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -28,7 +28,7 @@ using namespace VirtualRobot; using namespace SimDynamics; SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; //this->setCaption(QString("ShowRobot - KIT - Humanoids Group")); @@ -236,9 +236,9 @@ void SimDynamicsWindow::buildVisualization() viewer->addVisualization(dynamicsRobot, colModel); viewer->addVisualization(dynamicsObject, colModel); - for (size_t i = 0; i < dynamicsObjects.size(); i++) + for (const auto & dynamicsObject : dynamicsObjects) { - viewer->addVisualization(dynamicsObjects[i], colModel); + viewer->addVisualization(dynamicsObject, colModel); } if (dynamicsObject2) @@ -263,14 +263,14 @@ void SimDynamicsWindow::comVisu() { std::vector<RobotNodePtr> n = robot->getRobotNodes(); - for (size_t i = 0; i < n.size(); i++) + for (const auto & i : n) { SoSeparator* sep = new SoSeparator; comSep->addChild(sep); - Eigen::Matrix4f cp = dynamicsRobot->getComGlobal(n[i]); + Eigen::Matrix4f cp = dynamicsRobot->getComGlobal(i); sep->addChild(CoinVisualizationFactory::getMatrixTransformScaleMM2M(cp)); sep->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(5.0f)); - comVisuMap[n[i]] = sep; + comVisuMap[i] = sep; } } } @@ -319,12 +319,12 @@ void SimDynamicsWindow::updateJoints() UI.comboBoxRobotNode->clear(); std::vector<RobotNodePtr> nodes = robot->getRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - if (nodes[i]->isRotationalJoint() || nodes[i]->isTranslationalJoint()) + if (node->isRotationalJoint() || node->isTranslationalJoint()) { - robotNodes.push_back(nodes[i]); - QString qstr(nodes[i]->getName().c_str()); + robotNodes.push_back(node); + QString qstr(node->getName().c_str()); UI.comboBoxRobotNode->addItem(qstr); } } @@ -811,7 +811,7 @@ void SimDynamicsWindow::stopCB() SoSensorManager* sensor_mgr = SoDB::getSensorManager(); sensor_mgr->removeTimerSensor(timerSensor); delete timerSensor; - timerSensor = NULL; + timerSensor = nullptr; } viewer.reset(); @@ -831,17 +831,17 @@ void SimDynamicsWindow::updateContactVisu() std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> c = dynamicsWorld->getEngine()->getContacts(); - for (size_t i = 0; i < c.size(); i++) + for (auto & i : c) { - cout << "Contact: " << c[i].objectAName << " + " << c[i].objectBName << endl; + cout << "Contact: " << i.objectAName << " + " << i.objectBName << endl; SoSeparator* normal = new SoSeparator; SoMatrixTransform* m = new SoMatrixTransform; SbMatrix ma; ma.makeIdentity(); - ma.setTranslate(SbVec3f(c[i].posGlobalB(0), c[i].posGlobalB(1), c[i].posGlobalB(2))); + ma.setTranslate(SbVec3f(i.posGlobalB(0), i.posGlobalB(1), i.posGlobalB(2))); m->matrix.setValue(ma); normal->addChild(m); - SoSeparator* n = CoinVisualizationFactory::CreateArrow(c[i].normalGlobalB, 50.0f); + SoSeparator* n = CoinVisualizationFactory::CreateArrow(i.normalGlobalB, 50.0f); if (n) { @@ -851,10 +851,10 @@ void SimDynamicsWindow::updateContactVisu() SoSeparator* normal2 = new SoSeparator; SoMatrixTransform* m2 = new SoMatrixTransform; ma.makeIdentity(); - ma.setTranslate(SbVec3f(c[i].posGlobalA(0), c[i].posGlobalA(1), c[i].posGlobalA(2))); + ma.setTranslate(SbVec3f(i.posGlobalA(0), i.posGlobalA(1), i.posGlobalA(2))); m2->matrix.setValue(ma); normal2->addChild(m2); - SoSeparator* n2 = CoinVisualizationFactory::CreateArrow(-c[i].normalGlobalB, 50.0f); + SoSeparator* n2 = CoinVisualizationFactory::CreateArrow(-i.normalGlobalB, 50.0f); if (n2) { diff --git a/VirtualRobot/BoundingBox.cpp b/VirtualRobot/BoundingBox.cpp index 204855283ecc0231658b4f36f5ed12ca7aa06caa..1af12ffdddd741b8976444fb8c67466e78dc6924 100644 --- a/VirtualRobot/BoundingBox.cpp +++ b/VirtualRobot/BoundingBox.cpp @@ -147,10 +147,10 @@ namespace VirtualRobot result[7] << max(0), max(1), max(2); Eigen::Matrix4f m; - for (int i = 0; i < 8; i++) + for (const auto & i : result) { m.setIdentity(); - m.block(0, 3, 3, 1) = result[i]; + m.block(0, 3, 3, 1) = i; m = pose * m; result3.push_back(m.block(0, 3, 3, 1)); } @@ -176,7 +176,7 @@ namespace VirtualRobot } } - void BoundingBox::scale(Eigen::Vector3f& scaleFactor) + void BoundingBox::scale(const Eigen::Vector3f& scaleFactor) { if (std::isnan(min(0)) || std::isnan(min(1)) || std::isnan(min(2)) || std::isnan(max(0)) || std::isnan(max(1)) || std::isnan(max(2))) diff --git a/VirtualRobot/BoundingBox.h b/VirtualRobot/BoundingBox.h index d21eb02978ebef4fdf3c819af2ddaadde4e5a070..b510964362da01cf9462979e0bd6e94dd5d4e379 100644 --- a/VirtualRobot/BoundingBox.h +++ b/VirtualRobot/BoundingBox.h @@ -88,7 +88,7 @@ namespace VirtualRobot */ void transform(Eigen::Matrix4f& pose); - void scale(Eigen::Vector3f& scaleFactor); + void scale(const Eigen::Vector3f& scaleFactor); std::string toXML(int tabs = 2, bool skipMatrixTag = false); diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt index 75a0c60bac04b7ae04b0e46a8212af7c34b44bf2..0b57822fe62f07680a7577c66dd35b8a1a45e7ce 100644 --- a/VirtualRobot/CMakeLists.txt +++ b/VirtualRobot/CMakeLists.txt @@ -77,6 +77,10 @@ XML/RobotIO.cpp XML/SceneIO.cpp XML/ObjectIO.cpp XML/FileIO.cpp +XML/mjcf/MasslessBodySanitizer.cpp +XML/mjcf/MjcfDocument.cpp +XML/mjcf/MujocoIO.cpp +XML/mjcf/utils.cpp IK/IKSolver.cpp IK/AdvancedIKSolver.cpp IK/DifferentialIK.cpp @@ -137,17 +141,16 @@ VirtualRobotException.cpp ManipulationObject.cpp BoundingBox.cpp RuntimeEnvironment.cpp -Compression/CompressionRLE.cpp -Compression/CompressionBZip2.cpp +Random.cpp LinkedCoordinate.cpp SphereApproximator.cpp +VirtualRobot.cpp +Compression/CompressionRLE.cpp +Compression/CompressionBZip2.cpp Import/SimoxXMLFactory.cpp Import/RobotImporterFactory.cpp Import/MeshImport/STLReader.cpp -VirtualRobot.cpp Tools/Gravity.cpp -Random.cpp - math/Contact.cpp math/ContactList.cpp math/Line.cpp @@ -183,6 +186,7 @@ math/Primitive.cpp math/WeightedAverage.cpp math/Grid3D.cpp math/GridCacheFloat3.cpp +Util/xml/tinyxml2.cpp ) SET(INCLUDES @@ -223,6 +227,10 @@ XML/RobotIO.h XML/SceneIO.h XML/ObjectIO.h XML/FileIO.h +XML/mjcf/MasslessBodySanitizer.h +XML/mjcf/MjcfDocument.h +XML/mjcf/MujocoIO.h +XML/mjcf/utils.h IK/IKSolver.h IK/AdvancedIKSolver.h IK/DifferentialIK.h @@ -292,6 +300,7 @@ VirtualRobotTest.h ManipulationObject.h BoundingBox.h RuntimeEnvironment.h +Random.h DataStructures/nanoflann.hpp DataStructures/KdTreePointCloud.h Compression/CompressionRLE.h @@ -301,8 +310,6 @@ Import/SimoxXMLFactory.h Import/RobotImporterFactory.h Import/MeshImport/STLReader.h Tools/Gravity.h -Random.h - math/MathForwardDefinitions.h math/Contact.h math/ContactList.h @@ -347,7 +354,9 @@ math/Primitive.h math/Grid3D.h math/GridCacheFloat3.h math/WeightedAverage.h - +Util/xml/tinyxml2.h +Util/json/json.hpp +Util/json/eigen_conversion.hpp ) if (Simox_USE_RBDL AND RBDL_FOUND) @@ -551,6 +560,11 @@ ADD_LIBRARY (VirtualRobot SHARED ${SOURCES} ${INCLUDES}) TARGET_LINK_LIBRARIES(VirtualRobot PUBLIC ColCheckerPQP ${Simox_EXTERNAL_LIBRARIES}) target_include_directories(VirtualRobot SYSTEM PUBLIC ${Simox_EXTERNAL_INCLUDE_DIRS}) +# against undefined reference to boost::filesystem::detail::copy_file +# source: https://stackoverflow.com/a/3500721 +target_compile_definitions(${PROJECT_NAME} PRIVATE -DBOOST_NO_CXX11_SCOPED_ENUMS) + + # .DLL path SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR}) # .so path @@ -559,6 +573,8 @@ SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${Simo SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${Simox_LIB_DIR}) + + ####################################################################################### ############################ Setup for installation ################################### ####################################################################################### diff --git a/VirtualRobot/CollisionDetection/CDManager.cpp b/VirtualRobot/CollisionDetection/CDManager.cpp index bd44df1be45622c376b775331e57e90ae1f14702..aa854b6945dee20acdb94613ab1cf9437bfd9874 100644 --- a/VirtualRobot/CollisionDetection/CDManager.cpp +++ b/VirtualRobot/CollisionDetection/CDManager.cpp @@ -3,7 +3,7 @@ #include <iostream> #include <set> -#include <float.h> +#include <cfloat> #include "../Robot.h" @@ -16,7 +16,7 @@ namespace VirtualRobot CDManager::CDManager(CollisionCheckerPtr colChecker) { - if (colChecker == NULL) + if (colChecker == nullptr) { this->colChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker(); } @@ -27,8 +27,7 @@ namespace VirtualRobot } CDManager::~CDManager() - { - } + = default; void CDManager::addCollisionModel(SceneObjectSetPtr m) { @@ -39,11 +38,11 @@ namespace VirtualRobot VR_WARNING << "CollisionModel of SceneObjectSet '" << m->getName() << "' is linked to a different instance of collision checker than this CollisionManager..." << endl; } - for (size_t i = 0; i < colModels.size(); i++) + for (const auto & colModel : colModels) { - if (m != colModels[i]) + if (m != colModel) { - addCollisionModelPair(colModels[i], m); + addCollisionModelPair(colModel, m); } } @@ -73,11 +72,11 @@ namespace VirtualRobot } // check all colmodels - for (unsigned int i = 0; i < colModels.size(); i++) + for (const auto & colModel : colModels) { - if (m != colModels[i]) + if (m != colModel) { - if (colChecker->checkCollision(colModels[i], m)) + if (colChecker->checkCollision(colModel, m)) { return true; } @@ -101,11 +100,11 @@ namespace VirtualRobot } // check all colmodels - for (unsigned int i = 0; i < colModels.size(); i++) + for (const auto & colModel : colModels) { - if (m != colModels[i]) + if (m != colModel) { - tmp = (float)colChecker->calculateDistance(colModels[i], m); + tmp = (float)colChecker->calculateDistance(colModel, m); if (tmp < minDist) { @@ -123,9 +122,9 @@ namespace VirtualRobot { float minDist = FLT_MAX; - for (size_t i = 0; i < sets.size(); i++) + for (const auto & set : sets) { - float tmp = (float)colChecker->calculateDistance(m, sets[i]); + float tmp = (float)colChecker->calculateDistance(m, set); if (tmp < minDist) { @@ -172,9 +171,9 @@ namespace VirtualRobot int _trID1; int _trID2; - for (size_t i = 0; i < sets.size(); i++) + for (const auto & set : sets) { - float tmp = (float)colChecker->calculateDistance(m, sets[i], _P1, _P2, &_trID1, &_trID2); + float tmp = (float)colChecker->calculateDistance(m, set, _P1, _P2, &_trID1, &_trID2); if (tmp < minDist) { @@ -247,9 +246,9 @@ namespace VirtualRobot return -1.0f; } - for (unsigned int j = 0; j < colModels.size(); j++) + for (const auto & colModel : colModels) { - tmp = (float)colChecker->calculateDistance(m, colModels[j], _P1, _P2, &_trID1, &_trID2); + tmp = (float)colChecker->calculateDistance(m, colModel, _P1, _P2, &_trID1, &_trID2); if (tmp < minDist) { @@ -272,9 +271,9 @@ namespace VirtualRobot bool CDManager::isInCollision(SceneObjectSetPtr m, std::vector<SceneObjectSetPtr>& sets) { - for (size_t i = 0; i < sets.size(); i++) + for (const auto & set : sets) { - if (colChecker->checkCollision(m, sets[i])) + if (colChecker->checkCollision(m, set)) { return true; } @@ -318,9 +317,9 @@ namespace VirtualRobot bool CDManager::hasSceneObjectSet(SceneObjectSetPtr m) { - for (size_t i = 0; i < colModels.size(); i++) + for (const auto & colModel : colModels) { - if (colModels[i] == m) + if (colModel == m) { return true; } @@ -331,14 +330,14 @@ namespace VirtualRobot bool CDManager::_hasSceneObjectSet(SceneObjectSetPtr m) { - for (size_t i = 0; i < colModels.size(); i++) + for (auto & colModel : colModels) { - if (colModels[i] == m) + if (colModel == m) { return true; } - if (m->getSize() == 1 && colModels[i]->getSize() == 1 && colModels[i]->getSceneObject(0) == m->getSceneObject(0)) + if (m->getSize() == 1 && colModel->getSize() == 1 && colModel->getSceneObject(0) == m->getSceneObject(0)) { return true; } @@ -349,9 +348,9 @@ namespace VirtualRobot bool CDManager::hasSceneObject(SceneObjectPtr m) { - for (size_t i = 0; i < colModels.size(); i++) + for (auto & colModel : colModels) { - if (colModels[i]->getSize() == 1 && colModels[i]->getSceneObject(0) == m) + if (colModel->getSize() == 1 && colModel->getSceneObject(0) == m) { return true; } diff --git a/VirtualRobot/CollisionDetection/CollisionChecker.cpp b/VirtualRobot/CollisionDetection/CollisionChecker.cpp index bab06c9efab07e8db72db7dc8e1cd0495fd10d21..c6756425908b6000255e662cd5b660e8388d3fef 100644 --- a/VirtualRobot/CollisionDetection/CollisionChecker.cpp +++ b/VirtualRobot/CollisionDetection/CollisionChecker.cpp @@ -71,26 +71,25 @@ namespace VirtualRobot // class CollisionChecker destructor //---------------------------------------------------------------------- CollisionChecker::~CollisionChecker() - { - } + = default; float CollisionChecker::calculateDistance(SceneObjectSetPtr model1, SceneObjectSetPtr model2) { VR_ASSERT(model1 && model2); - return calculateDistance(model1, model2, tmpV1, tmpV2, NULL, NULL); + return calculateDistance(model1, model2, tmpV1, tmpV2, nullptr, nullptr); } float CollisionChecker::calculateDistance(CollisionModelPtr model1, SceneObjectSetPtr model2) { VR_ASSERT(model1 && model2); - return calculateDistance(model1, model2, tmpV1, tmpV2, NULL, NULL); + return calculateDistance(model1, model2, tmpV1, tmpV2, nullptr, nullptr); } float CollisionChecker::calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2) { VR_ASSERT(model1 && model2); - return calculateDistance(model1, model2, tmpV1, tmpV2, NULL, NULL); + return calculateDistance(model1, model2, tmpV1, tmpV2, nullptr, nullptr); } @@ -106,7 +105,7 @@ namespace VirtualRobot } else { - return calculateDistance(model1->getCollisionModel(), model2, tmpV1, tmpV2, NULL, NULL); + return calculateDistance(model1->getCollisionModel(), model2, tmpV1, tmpV2, nullptr, nullptr); } } @@ -134,7 +133,7 @@ namespace VirtualRobot } else { - return calculateDistance(model1->getCollisionModel(), model2->getCollisionModel(), tmpV1, tmpV2, NULL, NULL); + return calculateDistance(model1->getCollisionModel(), model2->getCollisionModel(), tmpV1, tmpV2, nullptr, nullptr); } } @@ -391,11 +390,11 @@ namespace VirtualRobot SceneObjectSetPtr result(new SceneObjectSet(r->getName(), shared_from_this())); std::vector<RobotNodePtr> cm = r->getRobotNodes(); - for (size_t i = 0; i < cm.size(); i++) + for (auto & i : cm) { - if (cm[i]->getCollisionModel()) + if (i->getCollisionModel()) { - result->addSceneObject(cm[i]); + result->addSceneObject(i); } } @@ -779,13 +778,13 @@ namespace VirtualRobot // bbox was hit, test all points... std::vector< Eigen::Vector3f > pts = colModel->getModelVeticesGlobal(); - for (std::vector< Eigen::Vector3f >::iterator i = pts.begin(); i != pts.end(); i++) + for (auto & pt : pts) { - if (MathTools::getDistancePointPlane(*i, p) <= maxDist) + if (MathTools::getDistancePointPlane(pt, p) <= maxDist) { MathTools::ContactPoint contact; contact.n = p.n; - contact.p = *i; + contact.p = pt; storeContatcs.push_back(contact); } } diff --git a/VirtualRobot/CollisionDetection/CollisionModel.cpp b/VirtualRobot/CollisionDetection/CollisionModel.cpp index 2fc51a4eb80194593a0b1a1501e37a69fff999fe..762d47d4d8ea48450e6eba5b803977a826ac2122 100644 --- a/VirtualRobot/CollisionDetection/CollisionModel.cpp +++ b/VirtualRobot/CollisionDetection/CollisionModel.cpp @@ -219,9 +219,9 @@ namespace VirtualRobot Eigen::Matrix4f t; t.setIdentity(); - for (std::vector<Eigen::Vector3f >::iterator i = model->vertices.begin(); i != model->vertices.end(); i++) + for (auto & vertice : model->vertices) { - t.block(0, 3, 3, 1) = *i; + t.block(0, 3, 3, 1) = vertice; t = globalPose * t; result.push_back(t.block(0, 3, 3, 1)); } @@ -351,16 +351,16 @@ namespace VirtualRobot CollisionCheckerPtr colChecker = colModels[0]->getCollisionChecker(); std::vector<VisualizationNodePtr> visus; - for (size_t i = 0; i < colModels.size(); i++) + for (const auto & colModel : colModels) { - VisualizationNodePtr v = colModels[i]->getVisualization(); + VisualizationNodePtr v = colModel->getVisualization(); if (v) { visus.push_back(v); } - VR_ASSERT(colModels[i]->getCollisionChecker() == colChecker); + VR_ASSERT(colModel->getCollisionChecker() == colChecker); } if (visus.size() == 0) diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp index 6bb10f4fc9bc524ade1d1709f71c9cd10322a86c..23bd8cd0609144ce6d3faebbe3500dae48f8913d 100644 --- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp +++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp @@ -35,7 +35,7 @@ namespace VirtualRobot CollisionCheckerPQP::~CollisionCheckerPQP() { delete pqpChecker; - pqpChecker = NULL; + pqpChecker = nullptr; } diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp index c7d8a4e34fc98fbd3df5dc62187abdd59ccf2250..e7a4c6911e9bb871e492b5c1e9960d8d63b97a83 100644 --- a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp +++ b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp @@ -29,15 +29,14 @@ namespace VirtualRobot } CollisionModelPQP::CollisionModelPQP(const CollisionModelPQP &orig) : - CollisionModelImplementation(orig.modelData, NULL, orig.id) + CollisionModelImplementation(orig.modelData, nullptr, orig.id) { pqpModel = orig.pqpModel; } CollisionModelPQP::~CollisionModelPQP() - { - } + = default; void CollisionModelPQP::destroyData() diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp index 9ad3ea6136e5c309faa8454353d80643dc6e7194..7242e75d8da7b2587fc33c864798f971ba7ab329 100644 --- a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp +++ b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.cpp @@ -38,37 +38,19 @@ \**************************************************************************/ -#include <stdlib.h> -#include <math.h> +#include <algorithm> + +#include <cstdlib> +#include <cmath> + #include "BV.h" #include "MatVec.h" #include "RectDist.h" #include "OBB_Disjoint.h" + namespace PQP { - - BV::BV() - { - first_child = 0; - } - - BV::~BV() - { - } - - inline - PQP_REAL - BV::MaxOfTwo(PQP_REAL a, PQP_REAL b) - { - if (a > b) - { - return a; - } - - return b; - } - void BV::FitToTris(PQP_REAL O[3][3], Tri* tris, int num_tris) { @@ -95,286 +77,272 @@ namespace PQP point++; } - PQP_REAL minx, maxx, miny, maxy, minz, maxz, c[3]; #if PQP_BV_TYPE & OBB_TYPE - minx = maxx = P[0][0]; - miny = maxy = P[0][1]; - minz = maxz = P[0][2]; - - for (i = 1; i < num_points; i++) { - if (P[i][0] < minx) - { - minx = P[i][0]; - } - else if (P[i][0] > maxx) - { - maxx = P[i][0]; - } + PQP_REAL minx, maxx, miny, maxy, minz, maxz, c[3]; + minx = maxx = P[0][0]; + miny = maxy = P[0][1]; + minz = maxz = P[0][2]; - if (P[i][1] < miny) + for (i = 1; i < num_points; i++) { - miny = P[i][1]; - } - else if (P[i][1] > maxy) - { - maxy = P[i][1]; - } + if (P[i][0] < minx) + { + minx = P[i][0]; + } + else if (P[i][0] > maxx) + { + maxx = P[i][0]; + } - if (P[i][2] < minz) - { - minz = P[i][2]; - } - else if (P[i][2] > maxz) - { - maxz = P[i][2]; + if (P[i][1] < miny) + { + miny = P[i][1]; + } + else if (P[i][1] > maxy) + { + maxy = P[i][1]; + } + + if (P[i][2] < minz) + { + minz = P[i][2]; + } + else if (P[i][2] > maxz) + { + maxz = P[i][2]; + } } - } - c[0] = (PQP_REAL)0.5 * (maxx + minx); - c[1] = (PQP_REAL)0.5 * (maxy + miny); - c[2] = (PQP_REAL)0.5 * (maxz + minz); - pqp_math.MxV(To, R, c); + c[0] = (PQP_REAL)0.5 * (maxx + minx); + c[1] = (PQP_REAL)0.5 * (maxy + miny); + c[2] = (PQP_REAL)0.5 * (maxz + minz); + pqp_math.MxV(To, R, c); - d[0] = (PQP_REAL)0.5 * (maxx - minx); - d[1] = (PQP_REAL)0.5 * (maxy - miny); - d[2] = (PQP_REAL)0.5 * (maxz - minz); + d[0] = (PQP_REAL)0.5 * (maxx - minx); + d[1] = (PQP_REAL)0.5 * (maxy - miny); + d[2] = (PQP_REAL)0.5 * (maxz - minz); + } #endif #if PQP_BV_TYPE & RSS_TYPE - - // compute thickness, which determines radius, and z of rectangle corner - - PQP_REAL cz, radsqr; - minz = maxz = P[0][2]; - - for (i = 1; i < num_points; i++) { - if (P[i][2] < minz) - { - minz = P[i][2]; - } - else if (P[i][2] > maxz) - { - maxz = P[i][2]; - } - } - - r = (PQP_REAL)0.5 * (maxz - minz); - radsqr = r * r; - cz = (PQP_REAL)0.5 * (maxz + minz); + PQP_REAL minx, maxx, miny, maxy, c[3]; + PQP_REAL cz, radsqr; - // compute an initial length of rectangle along x direction + // compute thickness, which determines radius, and z of rectangle corner + //AND + // compute an initial length of rectangle along x / y direction + // find minx and maxx / miny and maxy as starting points - // find minx and maxx as starting points - - int minindex, maxindex; - minindex = maxindex = 0; - - for (i = 1; i < num_points; i++) - { - if (P[i][0] < P[minindex][0]) { - minindex = i; - } - else if (P[i][0] > P[maxindex][0]) - { - maxindex = i; - } - } + int x_minindex = 0; + int x_maxindex = 0; + int y_minindex = 0; + int y_maxindex = 0; - PQP_REAL x, dz; - dz = P[minindex][2] - cz; - minx = P[minindex][0] + sqrt(MaxOfTwo(radsqr - dz * dz, 0)); - dz = P[maxindex][2] - cz; - maxx = P[maxindex][0] - sqrt(MaxOfTwo(radsqr - dz * dz, 0)); - - // grow minx - - for (i = 0; i < num_points; i++) - { - if (P[i][0] < minx) - { - dz = P[i][2] - cz; - x = P[i][0] + sqrt(MaxOfTwo(radsqr - dz * dz, 0)); - - if (x < minx) { - minx = x; - } - } - } + PQP_REAL minz = P[0][2]; + PQP_REAL maxz = P[0][2]; - // grow maxx - - for (i = 0; i < num_points; i++) - { - if (P[i][0] > maxx) - { - dz = P[i][2] - cz; - x = P[i][0] - sqrt(MaxOfTwo(radsqr - dz * dz, 0)); - - if (x > maxx) - { - maxx = x; + for (i = 1; i < num_points; i++) + { + if (P[i][0] < P[x_minindex][0]) + { + x_minindex = i; + } + else if (P[i][0] > P[x_maxindex][0]) + { + x_maxindex = i; + } + + if (P[i][1] < P[y_minindex][1]) + { + y_minindex = i; + } + else if (P[i][1] > P[y_maxindex][1]) + { + y_maxindex = i; + } + + if (P[i][2] < minz) + { + minz = P[i][2]; + } + else if (P[i][2] > maxz) + { + maxz = P[i][2]; + } + } + r = (PQP_REAL)0.5 * (maxz - minz); + radsqr = r * r; + cz = (PQP_REAL)0.5 * (maxz + minz); } - } - } - - // compute an initial length of rectangle along y direction - - // find miny and maxy as starting points - minindex = maxindex = 0; + PQP_REAL dz; + dz = P[x_minindex][2] - cz; + minx = P[x_minindex][0] + sqrt(std::max(radsqr - dz * dz, 0.f)); + dz = P[x_maxindex][2] - cz; + maxx = P[x_maxindex][0] - sqrt(std::max(radsqr - dz * dz, 0.f)); - for (i = 1; i < num_points; i++) - { - if (P[i][1] < P[minindex][1]) - { - minindex = i; + dz = P[y_minindex][2] - cz; + miny = P[y_minindex][1] + sqrt(std::max(radsqr - dz * dz, 0.f)); + dz = P[y_maxindex][2] - cz; + maxy = P[y_maxindex][1] - sqrt(std::max(radsqr - dz * dz, 0.f)); } - else if (P[i][1] > P[maxindex][1]) + + // grow minx / maxx / miny / maxy { - maxindex = i; - } - } + for (i = 0; i < num_points; i++) + { + // grow minx + if (P[i][0] < minx) + { + const PQP_REAL dz = P[i][2] - cz; + const PQP_REAL x = P[i][0] + sqrt(std::max(radsqr - dz * dz, 0.f)); - PQP_REAL y; - dz = P[minindex][2] - cz; - miny = P[minindex][1] + sqrt(MaxOfTwo(radsqr - dz * dz, 0)); - dz = P[maxindex][2] - cz; - maxy = P[maxindex][1] - sqrt(MaxOfTwo(radsqr - dz * dz, 0)); + if (x < minx) + { + minx = x; + } + } - // grow miny + // grow maxx + if (P[i][0] > maxx) + { + const PQP_REAL dz = P[i][2] - cz; + const PQP_REAL x = P[i][0] - sqrt(std::max(radsqr - dz * dz, 0.f)); - for (i = 0; i < num_points; i++) - { - if (P[i][1] < miny) - { - dz = P[i][2] - cz; - y = P[i][1] + sqrt(MaxOfTwo(radsqr - dz * dz, 0)); + if (x > maxx) + { + maxx = x; + } + } - if (y < miny) - { - miny = y; - } - } - } + // grow miny + if (P[i][1] < miny) + { + const PQP_REAL dz = P[i][2] - cz; + const PQP_REAL y = P[i][1] + sqrt(std::max(radsqr - dz * dz, 0.f)); - // grow maxy + if (y < miny) + { + miny = y; + } + } - for (i = 0; i < num_points; i++) - { - if (P[i][1] > maxy) - { - dz = P[i][2] - cz; - y = P[i][1] - sqrt(MaxOfTwo(radsqr - dz * dz, 0)); + // grow maxy + if (P[i][1] > maxy) + { + const PQP_REAL dz = P[i][2] - cz; + const PQP_REAL y = P[i][1] - sqrt(std::max(radsqr - dz * dz, 0.f)); - if (y > maxy) - { - maxy = y; + if (y > maxy) + { + maxy = y; + } + } } } - } - // corners may have some points which are not covered - grow lengths if - // necessary + // corners may have some points which are not covered - grow lengths if + // necessary - PQP_REAL dx, dy, u, t; - PQP_REAL a = sqrt((PQP_REAL)0.5); + PQP_REAL a = sqrt((PQP_REAL)0.5); - for (i = 0; i < num_points; i++) - { - if (P[i][0] > maxx) + for (i = 0; i < num_points; i++) { - if (P[i][1] > maxy) + if (P[i][0] > maxx) { - dx = P[i][0] - maxx; - dy = P[i][1] - maxy; - u = dx * a + dy * a; - t = (a * u - dx) * (a * u - dx) + - (a * u - dy) * (a * u - dy) + - (cz - P[i][2]) * (cz - P[i][2]); - u = u - sqrt(MaxOfTwo(radsqr - t, 0)); - - if (u > 0) + if (P[i][1] > maxy) { - maxx += u * a; - maxy += u * a; + const PQP_REAL dx = P[i][0] - maxx; + const PQP_REAL dy = P[i][1] - maxy; + PQP_REAL u = dx * a + dy * a; + const PQP_REAL t = (a * u - dx) * (a * u - dx) + + (a * u - dy) * (a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); + u = u - sqrt(std::max(radsqr - t, 0.f)); + + if (u > 0) + { + maxx += u * a; + maxy += u * a; + } } - } - else if (P[i][1] < miny) - { - dx = P[i][0] - maxx; - dy = P[i][1] - miny; - u = dx * a - dy * a; - t = (a * u - dx) * (a * u - dx) + - (-a * u - dy) * (-a * u - dy) + - (cz - P[i][2]) * (cz - P[i][2]); - u = u - sqrt(MaxOfTwo(radsqr - t, 0)); - - if (u > 0) + else if (P[i][1] < miny) { - maxx += u * a; - miny -= u * a; + const PQP_REAL dx = P[i][0] - maxx; + const PQP_REAL dy = P[i][1] - miny; + PQP_REAL u = dx * a - dy * a; + const PQP_REAL t = (a * u - dx) * (a * u - dx) + + (-a * u - dy) * (-a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); + u = u - sqrt(std::max(radsqr - t, 0.f)); + + if (u > 0) + { + maxx += u * a; + miny -= u * a; + } } } - } - else if (P[i][0] < minx) - { - if (P[i][1] > maxy) + else if (P[i][0] < minx) { - dx = P[i][0] - minx; - dy = P[i][1] - maxy; - u = dy * a - dx * a; - t = (-a * u - dx) * (-a * u - dx) + - (a * u - dy) * (a * u - dy) + - (cz - P[i][2]) * (cz - P[i][2]); - u = u - sqrt(MaxOfTwo(radsqr - t, 0)); - - if (u > 0) + if (P[i][1] > maxy) { - minx -= u * a; - maxy += u * a; + const PQP_REAL dx = P[i][0] - minx; + const PQP_REAL dy = P[i][1] - maxy; + PQP_REAL u = dy * a - dx * a; + const PQP_REAL t = (-a * u - dx) * (-a * u - dx) + + (a * u - dy) * (a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); + u = u - sqrt(std::max(radsqr - t, 0.f)); + + if (u > 0) + { + minx -= u * a; + maxy += u * a; + } } - } - else if (P[i][1] < miny) - { - dx = P[i][0] - minx; - dy = P[i][1] - miny; - u = -dx * a - dy * a; - t = (-a * u - dx) * (-a * u - dx) + - (-a * u - dy) * (-a * u - dy) + - (cz - P[i][2]) * (cz - P[i][2]); - u = u - sqrt(MaxOfTwo(radsqr - t, 0)); - - if (u > 0) + else if (P[i][1] < miny) { - minx -= u * a; - miny -= u * a; + const PQP_REAL dx = P[i][0] - minx; + const PQP_REAL dy = P[i][1] - miny; + PQP_REAL u = -dx * a - dy * a; + const PQP_REAL t = (-a * u - dx) * (-a * u - dx) + + (-a * u - dy) * (-a * u - dy) + + (cz - P[i][2]) * (cz - P[i][2]); + u = u - sqrt(std::max(radsqr - t, 0.f)); + + if (u > 0) + { + minx -= u * a; + miny -= u * a; + } } } } - } - c[0] = minx; - c[1] = miny; - c[2] = cz; - pqp_math.MxV(Tr, R, c); + c[0] = minx; + c[1] = miny; + c[2] = cz; + pqp_math.MxV(Tr, R, c); - l[0] = maxx - minx; + l[0] = maxx - minx; - if (l[0] < 0) - { - l[0] = 0; - } + if (l[0] < 0) + { + l[0] = 0; + } - l[1] = maxy - miny; + l[1] = maxy - miny; - if (l[1] < 0) - { - l[1] = 0; + if (l[1] < 0) + { + l[1] = 0; + } } #endif diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h index a05fcfdf12bfb1596e7a6b415e34f6587e866c9e..a5a39ff92d0d55db2fecf0356e52fa67c62cc659 100644 --- a/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h +++ b/VirtualRobot/CollisionDetection/PQP/PQP++/BV.h @@ -65,11 +65,11 @@ namespace PQP PQP_REAL d[3]; // (half) dimensions of obb #endif - int first_child; // positive value is index of first_child bv + int first_child{0}; // positive value is index of first_child bv // negative value is -(index + 1) of triangle - BV(); - ~BV(); + BV() = default; + ~BV() = default; int Leaf() { return first_child < 0; @@ -78,8 +78,6 @@ namespace PQP void FitToTris(PQP_REAL O[3][3], Tri* tris, int num_tris); MatVec pqp_math; - PQP_REAL MaxOfTwo(PQP_REAL a, PQP_REAL b); - }; inline diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/Build.cpp b/VirtualRobot/CollisionDetection/PQP/PQP++/Build.cpp index 64020b437cda26383f18462ea7ee38b604d72ad3..82c953ba4c38ff8cd863cc756af333f36515f894 100644 --- a/VirtualRobot/CollisionDetection/PQP/PQP++/Build.cpp +++ b/VirtualRobot/CollisionDetection/PQP/PQP++/Build.cpp @@ -38,9 +38,9 @@ \**************************************************************************/ -#include <stdio.h> -#include <stdlib.h> -#include <string.h> +#include <cstdio> +#include <cstdlib> +#include <cstring> #include "PQP.h" #include "Build.h" #include "MatVec.h" diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h b/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h index f37304f974beef0eda085d79d82c62333eb3fbc7..353c75ac63e6333187078a93faacc6753dd5bbda 100644 --- a/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h +++ b/VirtualRobot/CollisionDetection/PQP/PQP++/OBB_Disjoint.h @@ -40,6 +40,8 @@ #pragma once +#include <cmath> + #include "MatVec.h" #include "PQP_Compile.h" @@ -66,38 +68,33 @@ namespace PQP int obb_disjoint(PQP_REAL B[3][3], PQP_REAL T[3], PQP_REAL a[3], PQP_REAL b[3]) { - PQP_REAL t, s; + PQP_REAL s; int r; PQP_REAL Bf[3][3]; - const PQP_REAL reps = (PQP_REAL)1e-6; - - // Bf = fabs(B) - Bf[0][0] = myfabs(B[0][0]); - Bf[0][0] += reps; - Bf[0][1] = myfabs(B[0][1]); - Bf[0][1] += reps; - Bf[0][2] = myfabs(B[0][2]); - Bf[0][2] += reps; - Bf[1][0] = myfabs(B[1][0]); - Bf[1][0] += reps; - Bf[1][1] = myfabs(B[1][1]); - Bf[1][1] += reps; - Bf[1][2] = myfabs(B[1][2]); - Bf[1][2] += reps; - Bf[2][0] = myfabs(B[2][0]); - Bf[2][0] += reps; - Bf[2][1] = myfabs(B[2][1]); - Bf[2][1] += reps; - Bf[2][2] = myfabs(B[2][2]); - Bf[2][2] += reps; + + { + static constexpr PQP_REAL reps = (PQP_REAL)1e-6; + + // Bf = fabs(B) + Bf[0][0] = std::abs(B[0][0]) + reps; + Bf[0][1] = std::abs(B[0][1]) + reps; + Bf[0][2] = std::abs(B[0][2]) + reps; + + Bf[1][0] = std::abs(B[1][0]) + reps; + Bf[1][1] = std::abs(B[1][1]) + reps; + Bf[1][2] = std::abs(B[1][2]) + reps; + + Bf[2][0] = std::abs(B[2][0]) + reps; + Bf[2][1] = std::abs(B[2][1]) + reps; + Bf[2][2] = std::abs(B[2][2]) + reps; + } // if any of these tests are one-sided, then the polyhedra are disjoint r = 1; // A1 x A2 = A0 - t = myfabs(T[0]); - r &= (t <= + r &= (std::abs(T[0]) <= (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2])); if (!r) @@ -107,9 +104,8 @@ namespace PQP // B1 x B2 = B0 s = T[0] * B[0][0] + T[1] * B[1][0] + T[2] * B[2][0]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0])); if (!r) @@ -118,9 +114,8 @@ namespace PQP } // A2 x A0 = A1 - t = myfabs(T[1]); - r &= (t <= + r &= (std::abs(T[1]) <= (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2])); if (!r) @@ -129,9 +124,8 @@ namespace PQP } // A0 x A1 = A2 - t = myfabs(T[2]); - r &= (t <= + r &= (std::abs(T[2]) <= (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2])); if (!r) @@ -141,9 +135,8 @@ namespace PQP // B2 x B0 = B1 s = T[0] * B[0][1] + T[1] * B[1][1] + T[2] * B[2][1]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1])); if (!r) @@ -153,9 +146,8 @@ namespace PQP // B0 x B1 = B2 s = T[0] * B[0][2] + T[1] * B[1][2] + T[2] * B[2][2]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2])); if (!r) @@ -165,9 +157,8 @@ namespace PQP // A0 x B0 s = T[2] * B[1][0] - T[1] * B[2][0]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[1] * Bf[2][0] + a[2] * Bf[1][0] + b[1] * Bf[0][2] + b[2] * Bf[0][1])); @@ -178,9 +169,8 @@ namespace PQP // A0 x B1 s = T[2] * B[1][1] - T[1] * B[2][1]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[1] * Bf[2][1] + a[2] * Bf[1][1] + b[0] * Bf[0][2] + b[2] * Bf[0][0])); @@ -191,9 +181,8 @@ namespace PQP // A0 x B2 s = T[2] * B[1][2] - T[1] * B[2][2]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[1] * Bf[2][2] + a[2] * Bf[1][2] + b[0] * Bf[0][1] + b[1] * Bf[0][0])); @@ -204,9 +193,8 @@ namespace PQP // A1 x B0 s = T[0] * B[2][0] - T[2] * B[0][0]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[0] * Bf[2][0] + a[2] * Bf[0][0] + b[1] * Bf[1][2] + b[2] * Bf[1][1])); @@ -217,9 +205,8 @@ namespace PQP // A1 x B1 s = T[0] * B[2][1] - T[2] * B[0][1]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[0] * Bf[2][1] + a[2] * Bf[0][1] + b[0] * Bf[1][2] + b[2] * Bf[1][0])); @@ -230,9 +217,8 @@ namespace PQP // A1 x B2 s = T[0] * B[2][2] - T[2] * B[0][2]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[0] * Bf[2][2] + a[2] * Bf[0][2] + b[0] * Bf[1][1] + b[1] * Bf[1][0])); @@ -243,9 +229,8 @@ namespace PQP // A2 x B0 s = T[1] * B[0][0] - T[0] * B[1][0]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[0] * Bf[1][0] + a[1] * Bf[0][0] + b[1] * Bf[2][2] + b[2] * Bf[2][1])); @@ -256,9 +241,8 @@ namespace PQP // A2 x B1 s = T[1] * B[0][1] - T[0] * B[1][1]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[0] * Bf[1][1] + a[1] * Bf[0][1] + b[0] * Bf[2][2] + b[2] * Bf[2][0])); @@ -269,9 +253,8 @@ namespace PQP // A2 x B2 s = T[1] * B[0][2] - T[0] * B[1][2]; - t = myfabs(s); - r &= (t <= + r &= (std::abs(s) <= (a[0] * Bf[1][2] + a[1] * Bf[0][2] + b[0] * Bf[2][1] + b[1] * Bf[2][0])); diff --git a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp index 9cf945ddf279c745582fefa32a0a4d81cbb61485..6a8ef8f31635fc9d0003a61c6fbecbc0594fc9c6 100644 --- a/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp +++ b/VirtualRobot/CollisionDetection/PQP/PQP++/PQP.cpp @@ -38,8 +38,8 @@ \**************************************************************************/ -#include <stdio.h> -#include <string.h> +#include <cstdio> +#include <cstring> #include "PQP.h" #include "BVTQ.h" #include "Build.h" @@ -63,29 +63,29 @@ namespace PQP { // no bounding volume tree yet - b = 0; + b = nullptr; num_bvs_alloced = 0; num_bvs = 0; // no tri list yet - tris = 0; + tris = nullptr; num_tris = 0; num_tris_alloced = 0; - last_tri = 0; + last_tri = nullptr; build_state = PQP_BUILD_STATE_EMPTY; } PQP_Model::~PQP_Model() { - if (b != NULL) + if (b != nullptr) { delete [] b; } - if (tris != NULL) + if (tris != nullptr) { delete [] tris; } @@ -287,7 +287,7 @@ namespace PQP PQP_CollideResult::PQP_CollideResult() { - pairs = 0; + pairs = nullptr; num_pairs = num_pairs_alloced = 0; num_bv_tests = 0; num_tri_tests = 0; @@ -303,7 +303,7 @@ namespace PQP { num_pairs = num_pairs_alloced = 0; delete [] pairs; - pairs = 0; + pairs = nullptr; } // may increase OR reduce mem usage @@ -961,7 +961,7 @@ namespace PQP pqp_math.McM(min_test.R, R); pqp_math.VcV(min_test.T, T); - while (1) + while (true) { int l1 = o1->child(min_test.b1)->Leaf(); int l2 = o2->child(min_test.b2)->Leaf(); @@ -1378,7 +1378,7 @@ namespace PQP pqp_math.McM(min_test.R, R); pqp_math.VcV(min_test.T, T); - while (1) + while (true) { int l1 = o1->child(min_test.b1)->Leaf(); int l2 = o2->child(min_test.b2)->Leaf(); diff --git a/VirtualRobot/Compression/CompressionBZip2.cpp b/VirtualRobot/Compression/CompressionBZip2.cpp index 51375babff2a958daffc6d695f70848c7b459eb7..74a0604d7917f8eb81298cf1eaa9b03f7573a077 100644 --- a/VirtualRobot/Compression/CompressionBZip2.cpp +++ b/VirtualRobot/Compression/CompressionBZip2.cpp @@ -366,10 +366,10 @@ namespace VirtualRobot { this->mode = eCompress; this->ofs = ofs; - ifs = NULL; + ifs = nullptr; THROW_VR_EXCEPTION_IF(!ofs, "Stream NULL"); THROW_VR_EXCEPTION_IF(!this->ofs->good(), "Stream not good"); - bzFileData = NULL; + bzFileData = nullptr; bzFileData = BZ2_bzWriteOpen(¤tError, this->ofs); THROW_VR_EXCEPTION_IF(!bzFileData, "Could not initialize compression..."); } @@ -378,11 +378,11 @@ namespace VirtualRobot { this->mode = eUncompress; this->ifs = ifs; - ofs = NULL; + ofs = nullptr; THROW_VR_EXCEPTION_IF(!ifs, "Stream NULL"); THROW_VR_EXCEPTION_IF(!this->ifs->good(), "Stream not good"); - bzFileData = NULL; - bzFileData = BZ2_bzReadOpen(¤tError, this->ifs, 0, 0, NULL, 0); + bzFileData = nullptr; + bzFileData = BZ2_bzReadOpen(¤tError, this->ifs, 0, 0, nullptr, 0); THROW_VR_EXCEPTION_IF(!bzFileData, "Could not initialize compression..."); } @@ -398,10 +398,10 @@ namespace VirtualRobot { if (mode == eCompress && bzFileData && ofs) { - BZ2_bzWriteClose(¤tError, bzFileData, 0, NULL, NULL); + BZ2_bzWriteClose(¤tError, bzFileData, 0, nullptr, nullptr); //ofs->close(); //fclose (dataFile); - bzFileData = NULL; + bzFileData = nullptr; if (currentError < 0) //== BZ_IO_ERROR) { { @@ -412,7 +412,7 @@ namespace VirtualRobot else if (mode == eUncompress && bzFileData && ifs) { BZ2_bzReadClose(¤tError, bzFileData); - bzFileData = NULL; + bzFileData = nullptr; //fclose (dataFile); } @@ -2685,7 +2685,7 @@ zero: /*-- the log(N) loop --*/ H = 1; - while (1) + while (true) { if (verb >= 4) @@ -2715,7 +2715,7 @@ zero: nNotDone = 0; r = -1; - while (1) + while (true) { /*-- find the next non-singleton bucket --*/ @@ -2966,9 +2966,9 @@ zero: unLo = ltLo = lo; unHi = gtHi = hi; - while (1) + while (true) { - while (1) + while (true) { if (unLo > unHi) { @@ -2993,7 +2993,7 @@ zero: unLo++; } - while (1) + while (true) { if (unLo > unHi) { @@ -3204,9 +3204,9 @@ case lll: s->state = lll; \ s->save_zj = 0; s->save_gSel = 0; s->save_gMinlen = 0; - s->save_gLimit = NULL; - s->save_gBase = NULL; - s->save_gPerm = NULL; + s->save_gLimit = nullptr; + s->save_gBase = nullptr; + s->save_gPerm = nullptr; } /*restore from the save area*/ @@ -3278,7 +3278,7 @@ case lll: s->state = lll; \ ((1 + s->blockSize100k * 100000) >> 1) * sizeof(UChar) ); - if (s->ll16 == NULL || s->ll4 == NULL) + if (s->ll16 == nullptr || s->ll4 == nullptr) { RETURN(BZ_MEM_ERROR); } @@ -3287,7 +3287,7 @@ case lll: s->state = lll; \ { s->tt = (UInt32*)BZALLOC(s->blockSize100k * 100000 * sizeof(Int32)); - if (s->tt == NULL) + if (s->tt == nullptr) { RETURN(BZ_MEM_ERROR); } @@ -3993,31 +3993,31 @@ save_state_and_return: int workFactor) { Int32 ret; - bzFile* bzf = NULL; + bzFile* bzf = nullptr; BZ_SETERR(BZ_OK); - if (f == NULL || + if (f == nullptr || (blockSize100k < 1 || blockSize100k > 9) || (workFactor < 0 || workFactor > 250) || (verbosity < 0 || verbosity > 4)) { BZ_SETERR(BZ_PARAM_ERROR); - return NULL; + return nullptr; }; if (!f || f->bad()) { BZ_SETERR(BZ_IO_ERROR); - return NULL; + return nullptr; }; bzf = (bzFile*)malloc(sizeof(bzFile)); - if (bzf == NULL) + if (bzf == nullptr) { BZ_SETERR(BZ_MEM_ERROR); - return NULL; + return nullptr; }; BZ_SETERR(BZ_OK); @@ -4026,17 +4026,17 @@ save_state_and_return: bzf->bufN = 0; - bzf->handleIn = NULL; + bzf->handleIn = nullptr; bzf->handleOut = f; bzf->writing = BZ_True; - bzf->strm.bzalloc = NULL; + bzf->strm.bzalloc = nullptr; - bzf->strm.bzfree = NULL; + bzf->strm.bzfree = nullptr; - bzf->strm.opaque = NULL; + bzf->strm.opaque = nullptr; if (workFactor == 0) { @@ -4050,7 +4050,7 @@ save_state_and_return: { BZ_SETERR(ret); free(bzf); - return NULL; + return nullptr; }; bzf->strm.avail_in = 0; @@ -4075,7 +4075,7 @@ save_state_and_return: return BZ_CONFIG_ERROR; } - if (strm == NULL || + if (strm == nullptr || blockSize100k < 1 || blockSize100k > 9 || workFactor < 0 || workFactor > 250) { @@ -4087,52 +4087,52 @@ save_state_and_return: workFactor = 30; } - if (strm->bzalloc == NULL) + if (strm->bzalloc == nullptr) { strm->bzalloc = default_bzalloc; } - if (strm->bzfree == NULL) + if (strm->bzfree == nullptr) { strm->bzfree = default_bzfree; } s = (EState*)BZALLOC(sizeof(EState)); - if (s == NULL) + if (s == nullptr) { return BZ_MEM_ERROR; } s->strm = strm; - s->arr1 = NULL; - s->arr2 = NULL; - s->ftab = NULL; + s->arr1 = nullptr; + s->arr2 = nullptr; + s->ftab = nullptr; n = 100000 * blockSize100k; s->arr1 = (UInt32*)BZALLOC(n * sizeof(UInt32)); s->arr2 = (UInt32*)BZALLOC((n + BZ_N_OVERSHOOT) * sizeof(UInt32)); s->ftab = (UInt32*)BZALLOC(65537 * sizeof(UInt32)); - if (s->arr1 == NULL || s->arr2 == NULL || s->ftab == NULL) + if (s->arr1 == nullptr || s->arr2 == nullptr || s->ftab == nullptr) { - if (s->arr1 != NULL) + if (s->arr1 != nullptr) { BZFREE(s->arr1); } - if (s->arr2 != NULL) + if (s->arr2 != nullptr) { BZFREE(s->arr2); } - if (s->ftab != NULL) + if (s->ftab != nullptr) { BZFREE(s->ftab); } - if (s != NULL) + if (s != nullptr) { BZFREE(s); } @@ -4151,7 +4151,7 @@ save_state_and_return: s->block = (UChar*)s->arr2; s->mtfv = (UInt16*)s->arr1; - s->zbits = NULL; + s->zbits = nullptr; s->ptr = (UInt32*)s->arr1; strm->state = s; @@ -4165,7 +4165,7 @@ save_state_and_return: } /*---------------------------------------------------*/ - int CompressionBZip2::bz_config_ok(void) + int CompressionBZip2::bz_config_ok() { if (sizeof(int) != 4) { @@ -4235,7 +4235,7 @@ save_state_and_return: BZ_SETERR(BZ_OK); - if (bzf == NULL || buf == NULL || len < 0) + if (bzf == nullptr || buf == nullptr || len < 0) { BZ_SETERR(BZ_PARAM_ERROR); return; @@ -4308,7 +4308,7 @@ save_state_and_return: unsigned int* nbytes_out) { BZ2_bzWriteClose64(bzerror, b, abandon, - nbytes_in, NULL, nbytes_out, NULL); + nbytes_in, nullptr, nbytes_out, nullptr); } @@ -4324,7 +4324,7 @@ save_state_and_return: Int32 n, ret; bzFile* bzf = (bzFile*)b; - if (bzf == NULL) + if (bzf == nullptr) { BZ_SETERR(BZ_OK); return; @@ -4343,22 +4343,22 @@ save_state_and_return: return; }; - if (nbytes_in_lo32 != NULL) + if (nbytes_in_lo32 != nullptr) { *nbytes_in_lo32 = 0; } - if (nbytes_in_hi32 != NULL) + if (nbytes_in_hi32 != nullptr) { *nbytes_in_hi32 = 0; } - if (nbytes_out_lo32 != NULL) + if (nbytes_out_lo32 != nullptr) { *nbytes_out_lo32 = 0; } - if (nbytes_out_hi32 != NULL) + if (nbytes_out_hi32 != nullptr) { *nbytes_out_hi32 = 0; } @@ -4408,22 +4408,22 @@ save_state_and_return: }; } - if (nbytes_in_lo32 != NULL) + if (nbytes_in_lo32 != nullptr) { *nbytes_in_lo32 = bzf->strm.total_in_lo32; } - if (nbytes_in_hi32 != NULL) + if (nbytes_in_hi32 != nullptr) { *nbytes_in_hi32 = bzf->strm.total_in_hi32; } - if (nbytes_out_lo32 != NULL) + if (nbytes_out_lo32 != nullptr) { *nbytes_out_lo32 = bzf->strm.total_out_lo32; } - if (nbytes_out_hi32 != NULL) + if (nbytes_out_hi32 != nullptr) { *nbytes_out_hi32 = bzf->strm.total_out_hi32; } @@ -4438,14 +4438,14 @@ save_state_and_return: { EState* s; - if (strm == NULL) + if (strm == nullptr) { return BZ_PARAM_ERROR; } s = (EState*)strm->state; - if (s == NULL) + if (s == nullptr) { return BZ_PARAM_ERROR; } @@ -4455,24 +4455,24 @@ save_state_and_return: return BZ_PARAM_ERROR; } - if (s->arr1 != NULL) + if (s->arr1 != nullptr) { BZFREE(s->arr1); } - if (s->arr2 != NULL) + if (s->arr2 != nullptr) { BZFREE(s->arr2); } - if (s->ftab != NULL) + if (s->ftab != nullptr) { BZFREE(s->ftab); } BZFREE(strm->state); - strm->state = NULL; + strm->state = nullptr; return BZ_OK; } @@ -4482,14 +4482,14 @@ save_state_and_return: { DState* s; - if (strm == NULL) + if (strm == nullptr) { return BZ_PARAM_ERROR; } s = (DState*)strm->state; - if (s == NULL) + if (s == nullptr) { return BZ_PARAM_ERROR; } @@ -4499,23 +4499,23 @@ save_state_and_return: return BZ_PARAM_ERROR; } - if (s->tt != NULL) + if (s->tt != nullptr) { BZFREE(s->tt); } - if (s->ll16 != NULL) + if (s->ll16 != nullptr) { BZFREE(s->ll16); } - if (s->ll4 != NULL) + if (s->ll4 != nullptr) { BZFREE(s->ll4); } BZFREE(strm->state); - strm->state = NULL; + strm->state = nullptr; return BZ_OK; } @@ -4526,14 +4526,14 @@ save_state_and_return: Bool progress; EState* s; - if (strm == NULL) + if (strm == nullptr) { return BZ_PARAM_ERROR; } s = (EState*)strm->state; - if (s == NULL) + if (s == nullptr) { return BZ_PARAM_ERROR; } @@ -4878,34 +4878,34 @@ preswitch: void* unused, int nUnused) { - bzFile* bzf = NULL; + bzFile* bzf = nullptr; int ret; BZ_SETERR(BZ_OK); - if (f == NULL || + if (f == nullptr || (smallValue != 0 && smallValue != 1) || (verbosity < 0 || verbosity > 4) || - (unused == NULL && nUnused != 0) || - (unused != NULL && (nUnused < 0 || nUnused > BZ_MAX_UNUSED))) + (unused == nullptr && nUnused != 0) || + (unused != nullptr && (nUnused < 0 || nUnused > BZ_MAX_UNUSED))) { BZ_SETERR(BZ_PARAM_ERROR); - return NULL; + return nullptr; }; // if (ferror(f)) if (!f->good()) { BZ_SETERR(BZ_IO_ERROR); - return NULL; + return nullptr; }; bzf = (CompressionBZip2::bzFile*)malloc(sizeof(bzFile)); - if (bzf == NULL) + if (bzf == nullptr) { BZ_SETERR(BZ_MEM_ERROR); - return NULL; + return nullptr; }; BZ_SETERR(BZ_OK); @@ -4914,17 +4914,17 @@ preswitch: bzf->handleIn = f; - bzf->handleOut = NULL; + bzf->handleOut = nullptr; bzf->bufN = 0; bzf->writing = BZ_False; - bzf->strm.bzalloc = NULL; + bzf->strm.bzalloc = nullptr; - bzf->strm.bzfree = NULL; + bzf->strm.bzfree = nullptr; - bzf->strm.opaque = NULL; + bzf->strm.opaque = nullptr; while (nUnused > 0) { @@ -4940,7 +4940,7 @@ preswitch: { BZ_SETERR(ret); free(bzf); - return NULL; + return nullptr; }; bzf->strm.avail_in = bzf->bufN; @@ -4960,7 +4960,7 @@ preswitch: BZ_SETERR(BZ_OK); - if (bzf == NULL) + if (bzf == nullptr) { BZ_SETERR(BZ_OK); return; @@ -5018,7 +5018,7 @@ preswitch: BZ_SETERR(BZ_OK); - if (bzf == NULL || buf == NULL || len < 0) + if (bzf == nullptr || buf == nullptr || len < 0) { BZ_SETERR(BZ_PARAM_ERROR); return 0; @@ -5129,14 +5129,14 @@ preswitch: Bool corrupt; DState* s; - if (strm == NULL) + if (strm == nullptr) { return BZ_PARAM_ERROR; } s = (DState*)strm->state; - if (s == NULL) + if (s == nullptr) { return BZ_PARAM_ERROR; } @@ -5776,7 +5776,7 @@ return_notr: return BZ_CONFIG_ERROR; } - if (strm == NULL) + if (strm == nullptr) { return BZ_PARAM_ERROR; } @@ -5791,19 +5791,19 @@ return_notr: return BZ_PARAM_ERROR; } - if (strm->bzalloc == NULL) + if (strm->bzalloc == nullptr) { strm->bzalloc = default_bzalloc; } - if (strm->bzfree == NULL) + if (strm->bzfree == nullptr) { strm->bzfree = default_bzfree; } s = (DState*)BZALLOC(sizeof(DState)); - if (s == NULL) + if (s == nullptr) { return BZ_MEM_ERROR; } @@ -5819,9 +5819,9 @@ return_notr: strm->total_out_lo32 = 0; strm->total_out_hi32 = 0; s->smallDecompress = (Bool)smallValue; - s->ll4 = NULL; - s->ll16 = NULL; - s->tt = NULL; + s->ll4 = nullptr; + s->ll16 = nullptr; + s->tt = nullptr; s->currBlockNo = 0; s->verbosity = verbosity; @@ -5866,7 +5866,7 @@ return_notr: void CompressionBZip2::default_bzfree(void* /*opaque*/, void* addr) { - if (addr != NULL) + if (addr != nullptr) { free(addr); } diff --git a/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp b/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp index a97f3a637f5aba7de517c48a70fefd8eda9574d9..9b51d0a26e38452bb72d272d66bc7de889d63aa1 100644 --- a/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp +++ b/VirtualRobot/Compression/tests/CompressionBZip2Test.cpp @@ -13,14 +13,14 @@ #include <fstream> #include <istream> -#include <time.h> +#include <ctime> BOOST_AUTO_TEST_SUITE(Compression) BOOST_AUTO_TEST_CASE(testInvalidCreation) { - std::istream* is = NULL; - std::ostream* os = NULL; + std::istream* is = nullptr; + std::ostream* os = nullptr; BOOST_CHECK_THROW(new VirtualRobot::CompressionBZip2(is), VirtualRobot::VirtualRobotException); BOOST_CHECK_THROW(new VirtualRobot::CompressionBZip2(os), VirtualRobot::VirtualRobotException); } @@ -33,7 +33,7 @@ BOOST_AUTO_TEST_CASE(testNullBlock) std::stringstream ios; unsigned char blockN[BLOCK_SIZE_COMPRESSION_TEST]; memset(blockN, 0, sizeof(unsigned char)*BLOCK_SIZE_COMPRESSION_TEST); - VirtualRobot::CompressionBZip2* bzip2 = NULL; + VirtualRobot::CompressionBZip2* bzip2 = nullptr; BOOST_CHECK_NO_THROW(bzip2 = new VirtualRobot::CompressionBZip2((std::ostream*)(&ios))); bool ok = false; @@ -49,7 +49,7 @@ BOOST_AUTO_TEST_CASE(testNullBlock) ios.seekg(0); unsigned char blockN2[BLOCK_SIZE_COMPRESSION_TEST]; memset(blockN2, 1, sizeof(unsigned char)*BLOCK_SIZE_COMPRESSION_TEST); - VirtualRobot::CompressionBZip2* bzip2b = NULL; + VirtualRobot::CompressionBZip2* bzip2b = nullptr; BOOST_CHECK_NO_THROW(bzip2b = new VirtualRobot::CompressionBZip2((std::istream*)(&ios))); ok = false; @@ -62,9 +62,9 @@ BOOST_AUTO_TEST_CASE(testNullBlock) BOOST_CHECK_EQUAL(ios.eof(), true); - for (int i = 0; i < BLOCK_SIZE_COMPRESSION_TEST; i++) + for (unsigned char i : blockN2) { - BOOST_CHECK_EQUAL(blockN2[i], 0); + BOOST_CHECK_EQUAL(i, 0); } delete bzip2; @@ -90,14 +90,14 @@ BOOST_AUTO_TEST_CASE(testMultipleRandomBlocks) } } - VirtualRobot::CompressionBZip2* bzip2 = NULL; + VirtualRobot::CompressionBZip2* bzip2 = nullptr; BOOST_CHECK_NO_THROW(bzip2 = new VirtualRobot::CompressionBZip2((std::ostream*)(&ios))); bool ok = false; - for (int j = 0; j < NR_BLOCKS_COMPRESSION_TEST; j++) + for (auto & j : blockN) { - BOOST_CHECK_NO_THROW(ok = bzip2->write((void*)(blockN[j]), BLOCK_SIZE_COMPRESSION_TEST)); + BOOST_CHECK_NO_THROW(ok = bzip2->write((void*)j, BLOCK_SIZE_COMPRESSION_TEST)); BOOST_CHECK_EQUAL(ok, true); } @@ -109,15 +109,15 @@ BOOST_AUTO_TEST_CASE(testMultipleRandomBlocks) // set position to start ios.seekg(0); - VirtualRobot::CompressionBZip2* bzip2b = NULL; + VirtualRobot::CompressionBZip2* bzip2b = nullptr; BOOST_CHECK_NO_THROW(bzip2b = new VirtualRobot::CompressionBZip2((std::istream*)(&ios))); ok = false; int nrBytes = 0; - for (int j = 0; j < NR_BLOCKS_COMPRESSION_TEST; j++) + for (auto & j : blockN2) { - BOOST_CHECK_NO_THROW(ok = bzip2b->read((void*)(blockN2[j]), BLOCK_SIZE_COMPRESSION_TEST, nrBytes)); + BOOST_CHECK_NO_THROW(ok = bzip2b->read((void*)j, BLOCK_SIZE_COMPRESSION_TEST, nrBytes)); BOOST_CHECK_EQUAL(ok, true); BOOST_CHECK_EQUAL(nrBytes, BLOCK_SIZE_COMPRESSION_TEST); } @@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE(testCorrectEnding) std::stringstream ios; unsigned char blockN[sizeSmall]; memset(blockN, 0, sizeof(unsigned char)*sizeSmall); - VirtualRobot::CompressionBZip2* bzip2 = NULL; + VirtualRobot::CompressionBZip2* bzip2 = nullptr; BOOST_CHECK_NO_THROW(bzip2 = new VirtualRobot::CompressionBZip2((std::ostream*)(&ios))); bool ok = false; @@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(testCorrectEnding) ios.seekg(0); unsigned char blockN2[sizeSmall]; memset(blockN2, 1, sizeof(unsigned char)*sizeSmall); - VirtualRobot::CompressionBZip2* bzip2b = NULL; + VirtualRobot::CompressionBZip2* bzip2b = nullptr; BOOST_CHECK_NO_THROW(bzip2b = new VirtualRobot::CompressionBZip2((std::istream*)(&ios))); ok = false; @@ -181,9 +181,9 @@ BOOST_AUTO_TEST_CASE(testCorrectEnding) BOOST_CHECK_EQUAL(ok, true); - for (int i = 0; i < sizeSmall; i++) + for (unsigned char i : blockN2) { - BOOST_CHECK_EQUAL(blockN2[i], 0); + BOOST_CHECK_EQUAL(i, 0); } BOOST_CHECK_EQUAL(ios.eof(), false); diff --git a/VirtualRobot/EndEffector/EndEffector.cpp b/VirtualRobot/EndEffector/EndEffector.cpp index 5e2bfe366662924c64b4bff2688efd58cb0dffe3..b2b45c6d6cda387e54de5d011fcf14115c7033e3 100644 --- a/VirtualRobot/EndEffector/EndEffector.cpp +++ b/VirtualRobot/EndEffector/EndEffector.cpp @@ -38,16 +38,14 @@ namespace VirtualRobot gcpNode = tcpNode; } - for (size_t i = 0; i < preshapes.size(); i++) + for (const auto & preshape : preshapes) { - registerPreshape(preshapes[i]); + registerPreshape(preshape); } } EndEffector::~EndEffector() - { - - } + = default; EndEffectorPtr EndEffector::clone(RobotPtr newRobot) { @@ -115,12 +113,14 @@ namespace VirtualRobot EndEffector::ContactInfoVector EndEffector::closeActors(SceneObjectSetPtr obstacles, float stepSize) { - std::vector<bool> actorCollisionStatus(actors.size(), false); + std::vector<char> actorCollisionStatus(actors.size(), false); EndEffector::ContactInfoVector result; bool finished = false; int loop = 0; + const auto shared_this = shared_from_this(); + while (!finished) { loop++; @@ -133,10 +133,7 @@ namespace VirtualRobot { finished = false; - if (actors[i]->moveActorCheckCollision(shared_from_this(), result, obstacles, stepSize)) - { - actorCollisionStatus[i] = true; - } + actorCollisionStatus[i] = actors[i]->moveActorCheckCollision(shared_this, result, obstacles, stepSize); } } @@ -181,9 +178,9 @@ namespace VirtualRobot SceneObjectSetPtr cms(new SceneObjectSet(name, colChecker)); cms->addSceneObjects(statics); - for (std::vector<EndEffectorActorPtr>::iterator i = actors.begin(); i != actors.end(); i++) + for (auto & actor : actors) { - cms->addSceneObjects((*i)->getRobotNodes()); + cms->addSceneObjects(actor->getRobotNodes()); } return cms; @@ -261,16 +258,16 @@ namespace VirtualRobot cout << " * Static RobotNodes:" << endl; - for (size_t i = 0; i < statics.size(); i++) + for (auto & i : statics) { - cout << " ** " << statics[i]->getName() << endl; + cout << " ** " << i->getName() << endl; } cout << " * Actors:" << endl; - for (size_t i = 0; i < actors.size(); i++) + for (auto & actor : actors) { - actors[i]->print(); + actor->print(); } cout << endl; @@ -387,11 +384,11 @@ namespace VirtualRobot VirtualRobot::RobotConfigPtr result(new VirtualRobot::RobotConfig(getRobot(), getName())); std::vector< RobotNodePtr > rn = getAllNodes(); - for (size_t i = 0; i < rn.size(); i++) + for (auto & i : rn) { - if (rn[i]->isRotationalJoint() || rn[i]->isTranslationalJoint()) + if (i->isRotationalJoint() || i->isTranslationalJoint()) { - result->setConfig(rn[i]->getName(), rn[i]->getJointValue()); + result->setConfig(i->getName(), i->getJointValue()); } } @@ -416,9 +413,9 @@ namespace VirtualRobot { std::vector< RobotNodePtr > rn = (*iA)->getRobotNodes(); - for (size_t i = 0; i < rn.size(); i++) + for (const auto & i : rn) { - mapR[rn[i]] = rn[i]; + mapR[i] = i; } iA++; @@ -492,9 +489,9 @@ namespace VirtualRobot return false; } - for (size_t j = 0; j < actors.size(); j++) + for (const auto & actor : actors) { - if (!actors[j]->nodesSufficient(nodes)) + if (!actor->nodesSufficient(nodes)) { return false; } @@ -507,9 +504,9 @@ namespace VirtualRobot { float maxActor = 0; - for (size_t j = 0; j < actors.size(); j++) + for (auto & actor : actors) { - float al = actors[j]->getApproximatedLength(); + float al = actor->getApproximatedLength(); if (al > maxActor) { @@ -520,11 +517,11 @@ namespace VirtualRobot BoundingBox bb_all; - for (size_t j = 0; j < statics.size(); j++) + for (auto & j : statics) { - if (statics[j]->getCollisionModel()) + if (j->getCollisionModel()) { - BoundingBox bb = statics[j]->getCollisionModel()->getBoundingBox(); + BoundingBox bb = j->getCollisionModel()->getBoundingBox(); bb_all.addPoint(bb.getMin()); bb_all.addPoint(bb.getMax()); } @@ -609,18 +606,18 @@ namespace VirtualRobot { ss << tt << "<Static>" << endl; - for (size_t i = 0; i < statics.size(); i++) + for (auto & i : statics) { - ss << ttt << "<Node name='" << statics[i]->getName() << "'/>" << endl; + ss << ttt << "<Node name='" << i->getName() << "'/>" << endl; } ss << tt << "</Static>" << endl; } // Actors - for (size_t i = 0; i < actors.size(); i++) + for (auto & actor : actors) { - ss << actors[i]->toXML(ident + 1); + ss << actor->toXML(ident + 1); } ss << pre << "</EndEffector>" << endl; @@ -635,10 +632,8 @@ namespace VirtualRobot int contactCount = 0; - for (size_t i = 0; i < statics.size(); i++) + for (auto n : statics) { - RobotNodePtr n = statics[i]; - if (!n->getCollisionModel()) continue; int id1, id2; diff --git a/VirtualRobot/EndEffector/EndEffectorActor.cpp b/VirtualRobot/EndEffector/EndEffectorActor.cpp index 12f5fc2c12b951e4a305dd5f8bf381cc17097f10..a51a98fc0bbc28440a0f4dc292a39e69e911678f 100644 --- a/VirtualRobot/EndEffector/EndEffectorActor.cpp +++ b/VirtualRobot/EndEffector/EndEffectorActor.cpp @@ -46,12 +46,12 @@ namespace VirtualRobot std::vector<ActorDefinition> newDef; - for (unsigned int i = 0; i < actors.size(); i++) + for (auto & actor : actors) { ActorDefinition a; - a.colMode = actors[i].colMode; - a.directionAndSpeed = actors[i].directionAndSpeed; - a.robotNode = newRobot->getRobotNode(actors[i].robotNode->getName()); + a.colMode = actor.colMode; + a.directionAndSpeed = actor.directionAndSpeed; + a.robotNode = newRobot->getRobotNode(actor.robotNode->getName()); newDef.push_back(a); } @@ -74,13 +74,13 @@ namespace VirtualRobot VR_ASSERT(robot); bool res = true; - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - float v = n->robotNode->getJointValue() + angle * n->directionAndSpeed; + float v = actor.robotNode->getJointValue() + angle * actor.directionAndSpeed; - if (v <= n->robotNode->getJointLimitHi() && v >= n->robotNode->getJointLimitLo()) + if (v <= actor.robotNode->getJointLimitHi() && v >= actor.robotNode->getJointLimitLo()) { - robot->setJointValue(n->robotNode, v); + robot->setJointValue(actor.robotNode, v); //n->robotNode->setJointValue(v); res = false; } @@ -111,16 +111,16 @@ namespace VirtualRobot - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - float oldV = n->robotNode->getJointValue(); - float v = oldV + angle * n->directionAndSpeed; + float oldV = actor.robotNode->getJointValue(); + float v = oldV + angle * actor.directionAndSpeed; - actorStatus[n->robotNode] = eMoving; + actorStatus[actor.robotNode] = eMoving; - if (v <= n->robotNode->getJointLimitHi() && v >= n->robotNode->getJointLimitLo()) + if (v <= actor.robotNode->getJointLimitHi() && v >= actor.robotNode->getJointLimitLo()) { - robot->setJointValue(n->robotNode, v); + robot->setJointValue(actor.robotNode, v); //n->robotNode->setJointValue(v); // check collision @@ -135,10 +135,10 @@ namespace VirtualRobot // actors (don't store contacts) if (!collision) { - for (std::vector<EndEffectorActorPtr>::iterator a = eefActors.begin(); a != eefActors.end(); a++) + for (auto & eefActor : eefActors) { // Don't check for collisions with the actor itself (don't store contacts) - if (((*a)->getName() != name) && isColliding(*a)) //isColliding(eef,*a,newContacts) ) + if ((eefActor->getName() != name) && isColliding(eefActor)) //isColliding(eef,*a,newContacts) ) { collision = true; } @@ -148,9 +148,9 @@ namespace VirtualRobot // static (don't store contacts) if (!collision) { - for (std::vector<RobotNodePtr>::iterator node = eefStatic.begin(); node != eefStatic.end(); node++) + for (auto & node : eefStatic) { - SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(*node); + SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(node); //(don't store contacts) //if( isColliding(eef,so,newContacts,eStatic) ) @@ -169,25 +169,25 @@ namespace VirtualRobot { // reset last position //n->robotNode->setJointValue(oldV); - robot->setJointValue(n->robotNode, oldV); + robot->setJointValue(actor.robotNode, oldV); - actorStatus[n->robotNode] = eCollision; + actorStatus[actor.robotNode] = eCollision; } } else { - actorStatus[n->robotNode] = eFinished; + actorStatus[actor.robotNode] = eFinished; } } // update contacts - for (size_t i = 0; i < newContacts.size(); i++) + for (auto & newContact : newContacts) { // check for double entries (this may happen since we move all actors to the end and may detecting contacts multiple times) bool doubleEntry = false; - for (size_t j = 0; j < storeContacts.size(); j++) + for (auto & storeContact : storeContacts) { - if (storeContacts[j].robotNode == newContacts[i].robotNode && storeContacts[j].obstacle == newContacts[i].obstacle) + if (storeContact.robotNode == newContact.robotNode && storeContact.obstacle == newContact.obstacle) { doubleEntry = true; break; @@ -197,31 +197,31 @@ namespace VirtualRobot if (!doubleEntry) { int id1, id2; - newContacts[i].distance = colChecker->calculateDistance(newContacts[i].robotNode->getCollisionModel(), newContacts[i].obstacle->getCollisionModel(), newContacts[i].contactPointFingerGlobal, newContacts[i].contactPointObstacleGlobal, &id1, &id2); - newContacts[i].contactPointFingerLocal = newContacts[i].obstacle->toLocalCoordinateSystemVec(newContacts[i].contactPointFingerGlobal); - newContacts[i].contactPointObstacleLocal = newContacts[i].obstacle->toLocalCoordinateSystemVec(newContacts[i].contactPointObstacleGlobal); + newContact.distance = colChecker->calculateDistance(newContact.robotNode->getCollisionModel(), newContact.obstacle->getCollisionModel(), newContact.contactPointFingerGlobal, newContact.contactPointObstacleGlobal, &id1, &id2); + newContact.contactPointFingerLocal = newContact.obstacle->toLocalCoordinateSystemVec(newContact.contactPointFingerGlobal); + newContact.contactPointObstacleLocal = newContact.obstacle->toLocalCoordinateSystemVec(newContact.contactPointObstacleGlobal); // compute approach direction // todo: this could be done more elegantly (Jacobian) RobotConfigPtr config = getConfiguration(); - Eigen::Vector3f contGlobal1 = newContacts[i].contactPointFingerGlobal; - Eigen::Vector3f contFinger = newContacts[i].robotNode->toLocalCoordinateSystemVec(contGlobal1); + Eigen::Vector3f contGlobal1 = newContact.contactPointFingerGlobal; + Eigen::Vector3f contFinger = newContact.robotNode->toLocalCoordinateSystemVec(contGlobal1); this->moveActor(angle); - Eigen::Vector3f contGlobal2 = newContacts[i].robotNode->toGlobalCoordinateSystemVec(contFinger); - newContacts[i].approachDirectionGlobal = contGlobal2 - contGlobal1; - newContacts[i].approachDirectionGlobal.normalize(); + Eigen::Vector3f contGlobal2 = newContact.robotNode->toGlobalCoordinateSystemVec(contFinger); + newContact.approachDirectionGlobal = contGlobal2 - contGlobal1; + newContact.approachDirectionGlobal.normalize(); robot->setJointValues(config); - storeContacts.push_back(newContacts[i]); + storeContacts.push_back(newContact); } } // check what we should return bool res = true; - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { // if at least one actor is not in collision and not at its joint limits, we are still in the process of closing the fingers - if (actorStatus[n->robotNode] == eMoving) + if (actorStatus[actor.robotNode] == eMoving) res = false; } @@ -235,15 +235,15 @@ namespace VirtualRobot //Eigen::Vector3f contact; bool col = false; - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - for (std::vector<SceneObjectPtr>::iterator o = colModels.begin(); o != colModels.end(); o++) + for (auto & colModel : colModels) { - if ((n->colMode & checkColMode) && - ((*o)->getCollisionModel()) && - n->robotNode->getCollisionModel() && - colChecker->checkCollision(n->robotNode->getCollisionModel(), (*o)->getCollisionModel())) + if ((actor.colMode & checkColMode) && + (colModel->getCollisionModel()) && + actor.robotNode->getCollisionModel() && + colChecker->checkCollision(actor.robotNode->getCollisionModel(), colModel->getCollisionModel())) { col = true; @@ -251,8 +251,8 @@ namespace VirtualRobot EndEffector::ContactInfo ci; ci.eef = eef; ci.actor = shared_from_this(); - ci.robotNode = n->robotNode; - ci.obstacle = *o; + ci.robotNode = actor.robotNode; + ci.obstacle = colModel; // todo: maybe not needed here: we are in collision, distance makes no sense... // later the distance is calculated anyway (with slightly opened actors) @@ -271,9 +271,9 @@ namespace VirtualRobot bool EndEffectorActor::isColliding(SceneObjectSetPtr obstacles, CollisionMode checkColMode) { - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - if ((n->colMode & checkColMode) && n->robotNode->getCollisionModel() && colChecker->checkCollision(n->robotNode->getCollisionModel(), obstacles)) + if ((actor.colMode & checkColMode) && actor.robotNode->getCollisionModel() && colChecker->checkCollision(actor.robotNode->getCollisionModel(), obstacles)) { return true; } @@ -289,9 +289,9 @@ namespace VirtualRobot return false; } - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - if ((n->colMode & checkColMode) && n->robotNode->getCollisionModel() && colChecker->checkCollision(n->robotNode->getCollisionModel(), obstacle->getCollisionModel())) + if ((actor.colMode & checkColMode) && actor.robotNode->getCollisionModel() && colChecker->checkCollision(actor.robotNode->getCollisionModel(), obstacle->getCollisionModel())) { return true; } @@ -302,11 +302,11 @@ namespace VirtualRobot bool EndEffectorActor::isColliding(EndEffectorActorPtr obstacle) { - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(n->robotNode); + SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(actor.robotNode); - if ((n->colMode & eActors) && obstacle->isColliding(so)) + if ((actor.colMode & eActors) && obstacle->isColliding(so)) { return true; } @@ -323,18 +323,18 @@ namespace VirtualRobot std::vector<RobotNodePtr> obstacleStatics; obstacle->getStatics(obstacleStatics); - for (std::vector<EndEffectorActorPtr>::iterator actor = obstacleActors.begin(); actor != obstacleActors.end(); actor++) + for (auto & obstacleActor : obstacleActors) { // Don't check for collisions with the actor itself - if (((*actor)->getName() != name) && isColliding(*actor)) + if ((obstacleActor->getName() != name) && isColliding(obstacleActor)) { return true; } } - for (std::vector<RobotNodePtr>::iterator node = obstacleStatics.begin(); node != obstacleStatics.end(); node++) + for (auto & obstacleStatic : obstacleStatics) { - SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(*node); + SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(obstacleStatic); if (isColliding(so, EndEffectorActor::eStatic)) { @@ -353,18 +353,18 @@ namespace VirtualRobot std::vector<RobotNodePtr> obstacleStatics; obstacle->getStatics(obstacleStatics); - for (std::vector<EndEffectorActorPtr>::iterator actor = obstacleActors.begin(); actor != obstacleActors.end(); actor++) + for (auto & obstacleActor : obstacleActors) { // Don't check for collisions with the actor itself - if (((*actor)->getName() != name) && isColliding(eef, *actor, storeContacts)) + if ((obstacleActor->getName() != name) && isColliding(eef, obstacleActor, storeContacts)) { return true; } } - for (std::vector<RobotNodePtr>::iterator node = obstacleStatics.begin(); node != obstacleStatics.end(); node++) + for (auto & obstacleStatic : obstacleStatics) { - SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(*node); + SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(obstacleStatic); if (isColliding(eef, so, storeContacts, EndEffectorActor::eStatic)) { @@ -377,11 +377,11 @@ namespace VirtualRobot bool EndEffectorActor::isColliding(EndEffectorPtr eef, EndEffectorActorPtr obstacle, EndEffector::ContactInfoVector& storeContacts) { - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(n->robotNode); + SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(actor.robotNode); - if ((n->colMode & eActors) && obstacle->isColliding(eef, so, storeContacts)) + if ((actor.colMode & eActors) && obstacle->isColliding(eef, so, storeContacts)) { return true; } @@ -401,19 +401,19 @@ namespace VirtualRobot //Eigen::Vector3f contact; bool col = false; - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - if ((n->colMode & checkColMode) && - n->robotNode->getCollisionModel() && - colChecker->checkCollision(n->robotNode->getCollisionModel(), obstacle->getCollisionModel())) + if ((actor.colMode & checkColMode) && + actor.robotNode->getCollisionModel() && + colChecker->checkCollision(actor.robotNode->getCollisionModel(), obstacle->getCollisionModel())) { col = true; // create contact info EndEffector::ContactInfo ci; ci.eef = eef; ci.actor = shared_from_this(); - ci.robotNode = n->robotNode; + ci.robotNode = actor.robotNode; ci.obstacle = obstacle; // todo: not needed here, later we calculate the distance with opened actors... @@ -438,9 +438,9 @@ namespace VirtualRobot { std::vector< RobotNodePtr > res; - for (std::vector<ActorDefinition>::iterator n = actors.begin(); n != actors.end(); n++) + for (auto & actor : actors) { - res.push_back(n->robotNode); + res.push_back(actor.robotNode); } return res; @@ -451,9 +451,9 @@ namespace VirtualRobot cout << " ****" << endl; cout << " ** Name:" << name << endl; - for (size_t i = 0; i < actors.size(); i++) + for (auto & actor : actors) { - cout << " *** RobotNode: " << actors[i].robotNode->getName() << ", Direction/Speed:" << actors[i].directionAndSpeed << endl; + cout << " *** RobotNode: " << actor.robotNode->getName() << ", Direction/Speed:" << actor.directionAndSpeed << endl; //actors[i].robotNode->print(); } @@ -512,11 +512,11 @@ namespace VirtualRobot { BoundingBox bb_all; - for (size_t j = 0; j < actors.size(); j++) + for (auto & actor : actors) { - if (actors[j].robotNode->getCollisionModel()) + if (actor.robotNode->getCollisionModel()) { - BoundingBox bb = actors[j].robotNode->getCollisionModel()->getBoundingBox(); + BoundingBox bb = actor.robotNode->getCollisionModel()->getBoundingBox(); bb_all.addPoint(bb.getMin()); bb_all.addPoint(bb.getMax()); } @@ -535,11 +535,11 @@ namespace VirtualRobot std::vector< RobotConfig::Configuration > c; - for (size_t i = 0; i < actors.size(); i++) + for (auto & actor : actors) { RobotConfig::Configuration e; - e.name = actors[i].robotNode->getName(); - e.value = actors[i].robotNode->getJointValue(); + e.name = actor.robotNode->getName(); + e.value = actor.robotNode->getJointValue(); c.push_back(e); } @@ -562,33 +562,33 @@ namespace VirtualRobot std::string ttt = tt + t; ss << pre << "<Actor name='" << name << "'>" << endl; - for (size_t i = 0; i < actors.size(); i++) + for (auto & actor : actors) { - ss << tt << "<Node name='" << actors[i].robotNode->getName() << "' "; + ss << tt << "<Node name='" << actor.robotNode->getName() << "' "; - if (actors[i].colMode == eNone) + if (actor.colMode == eNone) { ss << "ConsiderCollisions='None' "; } - if (actors[i].colMode == eAll) + if (actor.colMode == eAll) { ss << "ConsiderCollisions='All' "; } else { - if (actors[i].colMode & eActors) + if (actor.colMode & eActors) { ss << "ConsiderCollisions='Actors' "; } - if (actors[i].colMode & eStatic) + if (actor.colMode & eStatic) { ss << "ConsiderCollisions='Static' "; } } - ss << "Direction='" << actors[i].directionAndSpeed << "'/>" << endl; + ss << "Direction='" << actor.directionAndSpeed << "'/>" << endl; } ss << pre << "</Actor>" << endl; diff --git a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp index 0811b6c34c5c33b1caedef08c3841eb5cab28c28..e39d7baa4647ce9b65217e4aeba169cf9b600d79 100644 --- a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp +++ b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp @@ -68,8 +68,7 @@ namespace VirtualRobot } BasicGraspQualityMeasure::~BasicGraspQualityMeasure() - { - } + = default; void BasicGraspQualityMeasure::setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& /*contactPoints6d*/) { @@ -99,31 +98,32 @@ namespace VirtualRobot { this->contactPoints.clear(); this->contactPointsM.clear(); - EndEffector::ContactInfoVector::const_iterator objPointsIter; - for (objPointsIter = contactPoints.begin(); objPointsIter != contactPoints.end(); objPointsIter++) + this->contactPoints.reserve(contactPoints.size()); + this->contactPointsM.reserve(contactPoints.size()); + + for (const auto& objPoint : contactPoints) { - MathTools::ContactPoint point; + this->contactPoints.emplace_back(); + MathTools::ContactPoint& point = this->contactPoints.back(); - point.p = objPointsIter->contactPointObstacleLocal; + point.p = objPoint.contactPointObstacleLocal; point.p -= centerOfModel; - point.n = objPointsIter->contactPointFingerLocal - objPointsIter->contactPointObstacleLocal; + point.n = objPoint.contactPointFingerLocal - objPoint.contactPointObstacleLocal; point.n.normalize(); // store force as projected component of approachDirection - Eigen::Vector3f nGlob = objPointsIter->contactPointObstacleGlobal - objPointsIter->contactPointFingerGlobal; + const Eigen::Vector3f nGlob = objPoint.contactPointObstacleGlobal - objPoint.contactPointFingerGlobal; if (nGlob.norm() > 1e-10) { - point.force = nGlob.dot(objPointsIter->approachDirectionGlobal) / nGlob.norm(); + point.force = nGlob.dot(objPoint.approachDirectionGlobal) / nGlob.norm(); } else { point.force = 0; } - - this->contactPoints.push_back(point); } VirtualRobot::MathTools::convertMM2M(this->contactPoints, this->contactPointsM); @@ -146,10 +146,10 @@ namespace VirtualRobot return p; } - for (int i = 0; i < (int)contactPoints.size(); i++) + for (auto & contactPoint : contactPoints) { - p.p += contactPoints[i].p; - p.n += contactPoints[i].n; + p.p += contactPoint.p; + p.n += contactPoint.n; } diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp index c91db04cebb86f113214b686c8db7b45bb0f94e5..e5150396b10ca75a52bb5e64f1586aad73b329ff 100644 --- a/VirtualRobot/Grasping/Grasp.cpp +++ b/VirtualRobot/Grasping/Grasp.cpp @@ -15,8 +15,7 @@ namespace VirtualRobot } Grasp::~Grasp() - { - } + = default; void Grasp::print(bool printDecoration /*= true*/) { diff --git a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp index 939768f4821383a21706a4f7ada93e3546b75c00..a3005f9dd79542efca86f045de47cf2b084999fb 100644 --- a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp +++ b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp @@ -14,7 +14,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -40,7 +40,7 @@ namespace VirtualRobot GraspEditorWindow::GraspEditorWindow(std::string& objFile, std::string& robotFile, bool embeddedGraspEditor) - : QMainWindow(NULL), UI(new Ui::MainWindowGraspEditor) + : QMainWindow(nullptr), UI(new Ui::MainWindowGraspEditor) { VR_INFO << " start " << endl; @@ -280,7 +280,7 @@ namespace VirtualRobot if (object) { - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; boost::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject = object->getVisualization<CoinVisualization>(colModel); if (visualizationObject) visualisationNode = visualizationObject->getCoinVisualization(); @@ -595,9 +595,9 @@ namespace VirtualRobot { UI->comboBoxEEF->clear(); - for (size_t i = 0; i < eefs.size(); i++) + for (auto & eef : eefs) { - UI->comboBoxEEF->addItem(QString(eefs[i]->getName().c_str())); + UI->comboBoxEEF->addItem(QString(eef->getName().c_str())); } } diff --git a/VirtualRobot/Grasping/GraspSet.cpp b/VirtualRobot/Grasping/GraspSet.cpp index 1703d6fae37f27dd3c9b7b39899aaf726ead7fe9..0b741dffb8eaaa90dabe021abca436c2f764adba 100644 --- a/VirtualRobot/Grasping/GraspSet.cpp +++ b/VirtualRobot/Grasping/GraspSet.cpp @@ -18,8 +18,7 @@ namespace VirtualRobot } GraspSet::~GraspSet() - { - } + = default; void GraspSet::addGrasp(GraspPtr grasp) { @@ -35,8 +34,8 @@ namespace VirtualRobot { VR_ASSERT_MESSAGE(grasp, "NULL grasp"); - for (size_t i = 0; i < grasps.size(); i++) - if (grasps[i] == grasp) + for (const auto & i : grasps) + if (i == grasp) { return true; } @@ -46,9 +45,9 @@ namespace VirtualRobot bool GraspSet::hasGrasp(const std::string& name) { - for (size_t i = 0; i < grasps.size(); i++) + for (auto & grasp : grasps) { - if (grasps[i]->getName() == name) + if (grasp->getName() == name) { return true; } @@ -67,11 +66,11 @@ namespace VirtualRobot { std::vector<GraspPtr> includedGrasp=grasps->getGrasps(); - for(size_t i=0;i<includedGrasp.size();i++) + for(const auto & i : includedGrasp) { - if(!hasGrasp(includedGrasp.at(i))) + if(!hasGrasp(i)) { - addGrasp(includedGrasp.at(i)); + addGrasp(i); } } } @@ -125,11 +124,11 @@ namespace VirtualRobot VirtualRobot::GraspPtr GraspSet::getGrasp(const std::string& name) { - for (size_t i = 0; i < grasps.size(); i++) + for (auto & grasp : grasps) { - if (grasps[i]->getName() == name) + if (grasp->getName() == name) { - return grasps[i]; + return grasp; } } @@ -164,9 +163,9 @@ namespace VirtualRobot ss << t << "<GraspSet name='" << name << "' RobotType='" << robotType << "' EndEffector='" << eef << "'>\n"; - for (size_t i = 0; i < grasps.size(); i++) + for (auto & grasp : grasps) { - ss << grasps[i]->toXML(tabs + 1); + ss << grasp->toXML(tabs + 1); } ss << t << "</GraspSet>\n"; @@ -180,9 +179,9 @@ namespace VirtualRobot GraspSetPtr res(new GraspSet(name, robotType, eef)); // clone grasps - for (std::vector< GraspPtr >::iterator i = grasps.begin(); i != grasps.end(); i++) + for (auto & grasp : grasps) { - res->addGrasp((*i)->clone()); + res->addGrasp(grasp->clone()); } return res; @@ -219,9 +218,9 @@ namespace VirtualRobot { std::vector< GraspPtr > res; - for (size_t i = 0; i < grasps.size(); i++) + for (const auto & grasp : grasps) { - res.push_back(grasps[i]); + res.push_back(grasp); } return res; @@ -229,9 +228,9 @@ namespace VirtualRobot void GraspSet::setPreshape(const std::string& preshape) { - for (size_t i = 0; i < grasps.size(); i++) + for (auto & grasp : grasps) { - grasps[i]->setPreshape(preshape); + grasp->setPreshape(preshape); } } diff --git a/VirtualRobot/IK/AdvancedIKSolver.cpp b/VirtualRobot/IK/AdvancedIKSolver.cpp index 6584353290072bd32494e321cfa51d4a26e9761a..a91953ec03263f10fe633c2034e3286247de97e0 100644 --- a/VirtualRobot/IK/AdvancedIKSolver.cpp +++ b/VirtualRobot/IK/AdvancedIKSolver.cpp @@ -107,9 +107,9 @@ namespace VirtualRobot robot->getEndEffectors(eefs); EndEffectorPtr eef; - for (size_t i = 0; i < eefs.size(); i++) + for (auto & i : eefs) { - if (eefs[i]->getTcp() == rns->getTCP()) + if (i->getTcp() == rns->getTCP()) { if (eef) { @@ -117,7 +117,7 @@ namespace VirtualRobot } else { - eef = eefs[i]; + eef = i; } } } diff --git a/VirtualRobot/IK/CoMIK.cpp b/VirtualRobot/IK/CoMIK.cpp index 49007581c3b341b5ce6f941a2426a3f21e427dd4..d68aef43422ba3966183b955ed3e7c00595c9f5c 100644 --- a/VirtualRobot/IK/CoMIK.cpp +++ b/VirtualRobot/IK/CoMIK.cpp @@ -5,7 +5,7 @@ #include "../VirtualRobotException.h" #include "../Robot.h" -#include <float.h> +#include <cfloat> namespace VirtualRobot { @@ -22,14 +22,14 @@ namespace VirtualRobot bodyNodes = rnsBodies->getAllRobotNodes(); - for (size_t i = 0; i < bodyNodes.size(); i++) + for (auto & bodyNode : bodyNodes) { // get all joints that influence the body - std::vector<RobotNodePtr> parentsN = bodyNodes[i]->getAllParents(rns); + std::vector<RobotNodePtr> parentsN = bodyNode->getAllParents(rns); // maybe this node is joint and body - if (rnsJoints->hasRobotNode(bodyNodes[i])) - parentsN.push_back(bodyNodes[i]); - bodyNodeParents[bodyNodes[i]] = parentsN; + if (rnsJoints->hasRobotNode(bodyNode)) + parentsN.push_back(bodyNode); + bodyNodeParents[bodyNode] = parentsN; } if (rnsBodies->getMass() == 0) diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp index 2558d30308189d48549d4f25ca1884d9e0a66a6a..4cb382d6eaaa81d1217adf2e5f0299aef6df47ae 100644 --- a/VirtualRobot/IK/DifferentialIK.cpp +++ b/VirtualRobot/IK/DifferentialIK.cpp @@ -9,7 +9,7 @@ #include <algorithm> -#include <float.h> +#include <cfloat> //#define CHECK_PERFORMANCE @@ -30,11 +30,11 @@ namespace VirtualRobot checkImprovement = false; nodes = rns->getAllRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - std::vector<RobotNodePtr> p = nodes[i]->getAllParents(rns); - p.push_back(nodes[i]);// if the tcp is not fixed, it must be considered for calculating the Jacobian - parents[nodes[i]] = p; + std::vector<RobotNodePtr> p = node->getAllParents(rns); + p.push_back(node);// if the tcp is not fixed, it must be considered for calculating the Jacobian + parents[node] = p; } convertMMtoM = false; @@ -122,10 +122,8 @@ namespace VirtualRobot size_t index = 0; - for (size_t i = 0; i < tcp_set.size(); i++) + for (auto tcp : tcp_set) { - SceneObjectPtr tcp = tcp_set[i]; - if (this->targets.find(tcp) != this->targets.end()) { auto& mode = this->modes[tcp]; @@ -186,10 +184,8 @@ namespace VirtualRobot // compute error size_t index = 0; - for (size_t i = 0; i < tcp_set.size(); i++) + for (auto tcp : tcp_set) { - SceneObjectPtr tcp = tcp_set[i]; - if (this->targets.find(tcp) != this->targets.end()) { //Eigen::VectorXf delta = @@ -553,9 +549,8 @@ namespace VirtualRobot partJacobians.clear(); - for (size_t i = 0; i < tcp_set.size(); i++) + for (auto tcp : tcp_set) { - SceneObjectPtr tcp = tcp_set[i]; int partSize = 0; if (this->modes[tcp] & IKSolver::X) @@ -613,9 +608,8 @@ namespace VirtualRobot cout << "TCPs:" << endl; - for (size_t t = 0; t < tcp_set.size(); t++) + for (auto tcp : tcp_set) { - SceneObjectPtr tcp = tcp_set[t]; RobotNodePtr tcpRN = boost::dynamic_pointer_cast<RobotNode>(tcp); if (!tcpRN) @@ -830,9 +824,8 @@ namespace VirtualRobot float res = 0; - for (size_t i = 0; i < tcp_set.size(); i++) + for (auto tcp : tcp_set) { - SceneObjectPtr tcp = tcp_set[i]; res += getErrorPosition(tcp); } @@ -844,9 +837,8 @@ namespace VirtualRobot { bool result = true; - for (size_t i = 0; i < tcp_set.size(); i++) + for (auto tcp : tcp_set) { - SceneObjectPtr tcp = tcp_set[i]; float currentErrorPos = getErrorPosition(tcp); float maxErrorPos = tolerancePosition[tcp]; float currentErrorRot = getErrorRotation(tcp); diff --git a/VirtualRobot/IK/FeetPosture.cpp b/VirtualRobot/IK/FeetPosture.cpp index c8c0eaa9d2adfb41000ceb3d84dca3dfa5d3b73a..3b3b02d3e1a82ff1cabebd65a4a0f9a1646ad21c 100644 --- a/VirtualRobot/IK/FeetPosture.cpp +++ b/VirtualRobot/IK/FeetPosture.cpp @@ -41,9 +41,7 @@ namespace VirtualRobot } FeetPosture::~FeetPosture() - { - - } + = default; RobotNodeSetPtr FeetPosture::getLeftLeg() { diff --git a/VirtualRobot/IK/GazeIK.cpp b/VirtualRobot/IK/GazeIK.cpp index 61226fe3392b1fb09fc6d73a11a814b69d0f8f02..bd66a54c81eee8e7b4b2bb57ad3909c45e6a14dc 100644 --- a/VirtualRobot/IK/GazeIK.cpp +++ b/VirtualRobot/IK/GazeIK.cpp @@ -5,7 +5,7 @@ #include <VirtualRobot/Random.h> #include <algorithm> -#include <float.h> +#include <cfloat> using namespace VirtualRobot; using namespace std; diff --git a/VirtualRobot/IK/HierarchicalIK.cpp b/VirtualRobot/IK/HierarchicalIK.cpp index 8c24d07677fbae1dab647e5d1f480caf7620ec6e..233c0ba70d265e1e7819f9c1da903e9f07d02000 100644 --- a/VirtualRobot/IK/HierarchicalIK.cpp +++ b/VirtualRobot/IK/HierarchicalIK.cpp @@ -15,9 +15,7 @@ namespace VirtualRobot } HierarchicalIK::~HierarchicalIK() - { - - } + = default; void HierarchicalIK::setVerbose(bool v) { diff --git a/VirtualRobot/IK/HierarchicalIKSolver.cpp b/VirtualRobot/IK/HierarchicalIKSolver.cpp index 8cf7ea9b87baef9f92052815757032957eea24d9..3dfec0767983e71f1f7a8934ee00b901a1a1f1dd 100644 --- a/VirtualRobot/IK/HierarchicalIKSolver.cpp +++ b/VirtualRobot/IK/HierarchicalIKSolver.cpp @@ -61,9 +61,9 @@ bool HierarchicalIKSolver::computeSteps(float stepSize, float minChange, int max bool HierarchicalIKSolver::checkTolerances() { - for (size_t i = 0; i < jacobies.size(); i++) + for (auto & jacobie : jacobies) { - if (!jacobies[i]->checkTolerances()) + if (!jacobie->checkTolerances()) { return false; } diff --git a/VirtualRobot/IK/IKSolver.cpp b/VirtualRobot/IK/IKSolver.cpp index 29359e68c19d27d27d87fd668bf6f2a5c06590b8..3720d663ba58d435d5dc23f2416f6b282c6b8584 100644 --- a/VirtualRobot/IK/IKSolver.cpp +++ b/VirtualRobot/IK/IKSolver.cpp @@ -3,7 +3,6 @@ namespace VirtualRobot { IKSolver::IKSolver() - { - } + = default; } // namespace VirtualRobot diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp index 6d1a2d410683185702e14232ce8815ca7e0c7f57..5ae8f2dd633623e98d53dfa5472d6767a7b51ad3 100644 --- a/VirtualRobot/IK/JacobiProvider.cpp +++ b/VirtualRobot/IK/JacobiProvider.cpp @@ -22,8 +22,7 @@ namespace VirtualRobot } JacobiProvider::~JacobiProvider() - { - } + = default; MatrixXd JacobiProvider::getJacobianMatrixD() { diff --git a/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp b/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp index b680691f94ffe57ff226489a0c8fb74019233b14..e886a5c6e3054b21886b50753151c88f992a617f 100644 --- a/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp +++ b/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp @@ -3,7 +3,7 @@ #include <Eigen/Geometry> #include <Eigen/Dense> -#include <float.h> +#include <cfloat> @@ -28,8 +28,7 @@ namespace VirtualRobot } PoseQualityExtendedManipulability::~PoseQualityExtendedManipulability() - { - } + = default; float PoseQualityExtendedManipulability::getPoseQuality() { diff --git a/VirtualRobot/IK/PoseQualityManipulability.cpp b/VirtualRobot/IK/PoseQualityManipulability.cpp index 6a098301cec81903fc8444891915e75ede95f71d..5b9cbbd9c2b6701f89a68c8c34674dbcd1fb97ea 100644 --- a/VirtualRobot/IK/PoseQualityManipulability.cpp +++ b/VirtualRobot/IK/PoseQualityManipulability.cpp @@ -21,8 +21,7 @@ namespace VirtualRobot PoseQualityManipulability::~PoseQualityManipulability() - { - } + = default; Eigen::MatrixXf PoseQualityManipulability::getSingularVectorCartesian() diff --git a/VirtualRobot/IK/PoseQualityMeasurement.cpp b/VirtualRobot/IK/PoseQualityMeasurement.cpp index 687bb2e3c132b5b93a388059863c386c0e6beea3..fa1d0116d0aeee89cdbd14ba3bab189c91efad66 100644 --- a/VirtualRobot/IK/PoseQualityMeasurement.cpp +++ b/VirtualRobot/IK/PoseQualityMeasurement.cpp @@ -21,8 +21,7 @@ namespace VirtualRobot PoseQualityMeasurement::~PoseQualityMeasurement() - { - } + = default; float PoseQualityMeasurement::getPoseQuality() { diff --git a/VirtualRobot/IK/StackedIK.cpp b/VirtualRobot/IK/StackedIK.cpp index 992f0ca96d3a3930ac1489db726a1765f86fd646..2ab7969f17eff6c638cbd16774a6362f0da57978 100644 --- a/VirtualRobot/IK/StackedIK.cpp +++ b/VirtualRobot/IK/StackedIK.cpp @@ -36,8 +36,7 @@ namespace VirtualRobot } StackedIK::~StackedIK() - { - } + = default; void StackedIK::setVerbose(bool v) { @@ -71,11 +70,11 @@ namespace VirtualRobot int current_row = 0; - for (size_t i = 0; i < jacDefs.size(); i++) + for (const auto & jacDef : jacDefs) { - Eigen::MatrixXf J = jacDefs[i]->getJacobianMatrix(); + Eigen::MatrixXf J = jacDef->getJacobianMatrix(); jacobian.block(current_row, 0, J.rows(), J.cols()) = J; - error.block(current_row, 0, J.rows(), 1) = jacDefs[i]->getError(); + error.block(current_row, 0, J.rows(), 1) = jacDef->getError(); current_row += J.rows(); } diff --git a/VirtualRobot/IK/SupportPolygon.cpp b/VirtualRobot/IK/SupportPolygon.cpp index b0ae8a139b4973fbcd113c0aa7985448f0d3c766..c47b57536d8ae63f942433d053a52600ba25b1b4 100644 --- a/VirtualRobot/IK/SupportPolygon.cpp +++ b/VirtualRobot/IK/SupportPolygon.cpp @@ -38,10 +38,10 @@ namespace VirtualRobot currentContactPoints2D.clear(); - for (size_t u = 0; u < points.size(); u++) + for (auto & point : points) { - Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(points[u].p, floor); + Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(point.p, floor); currentContactPoints2D.push_back(pt2d); } currentContactPoints2D = MathTools::sortPoints(currentContactPoints2D); diff --git a/VirtualRobot/IK/constraints/CoMConstraint.cpp b/VirtualRobot/IK/constraints/CoMConstraint.cpp index 982efbd9de3383b88196266cacca51d35c685f89..debe401019e49482c98bdede6c3097669c83d03e 100644 --- a/VirtualRobot/IK/constraints/CoMConstraint.cpp +++ b/VirtualRobot/IK/constraints/CoMConstraint.cpp @@ -116,5 +116,5 @@ bool CoMConstraint::checkTolerances() return d.norm() <= tolerance; } - return 0; + return false; } diff --git a/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.cpp b/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.cpp index b7c4803c4400a97c3da085d0f073081e2b15df09..8f9eb1012619264adbcf39d4e9b31645208044cd 100644 --- a/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.cpp +++ b/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.cpp @@ -15,19 +15,19 @@ #include "pugixml.hpp" -#include <stdlib.h> -#include <stdio.h> -#include <string.h> -#include <assert.h> -#include <limits.h> +#include <cstdlib> +#include <cstdio> +#include <cstring> +#include <cassert> +#include <climits> #ifdef PUGIXML_WCHAR_MODE # include <wchar.h> #endif #ifndef PUGIXML_NO_XPATH -# include <math.h> -# include <float.h> +# include <cmath> +# include <cfloat> # ifdef PUGIXML_NO_EXCEPTIONS # include <setjmp.h> # endif @@ -150,7 +150,7 @@ namespace pugi typedef unsigned __int32 uint32_t; } #else -# include <stdint.h> +# include <cstdint> #endif // Memory allocation @@ -177,7 +177,7 @@ PUGI__NS_BEGIN template <typename T> allocation_function xml_memory_management_function_storage<T>::allocate = default_allocate; template <typename T> deallocation_function xml_memory_management_function_storage<T>::deallocate = default_deallocate; - typedef xml_memory_management_function_storage<int> xml_memory; + using xml_memory = xml_memory_management_function_storage<int>; PUGI__NS_END // String utilities @@ -250,7 +250,7 @@ PUGI__NS_BEGIN T* release() { T* result = data; - data = 0; + data = nullptr; return result; } }; @@ -443,9 +443,9 @@ PUGI__NS_BEGIN { xml_memory_page* result = static_cast<xml_memory_page*>(memory); - result->allocator = 0; - result->prev = 0; - result->next = 0; + result->allocator = nullptr; + result->prev = nullptr; + result->next = nullptr; result->busy_size = 0; result->freed_size = 0; @@ -494,7 +494,7 @@ PUGI__NS_BEGIN // allocate block with some alignment, leaving memory for worst-case padding void* memory = xml_memory::allocate(size); - if (!memory) return 0; + if (!memory) return nullptr; // prepare page structure xml_memory_page* page = xml_memory_page::construct(memory); @@ -576,7 +576,7 @@ PUGI__NS_BEGIN if (page->freed_size == page->busy_size) { - if (page->next == 0) + if (page->next == nullptr) { assert(_root == page); @@ -623,7 +623,7 @@ PUGI__NS_BEGIN xml_memory_page* page; xml_memory_string_header* header = static_cast<xml_memory_string_header*>(allocate_memory(full_size, page)); - if (!header) return 0; + if (!header) return nullptr; // setup header ptrdiff_t page_offset = reinterpret_cast<char*>(header) - reinterpret_cast<char*>(page) - sizeof(xml_memory_page); @@ -685,7 +685,7 @@ PUGI__NS_BEGIN xml_memory_page* page = allocate_page(size <= large_allocation_threshold ? xml_memory_page_size : size); out_page = page; - if (!page) return 0; + if (!page) return nullptr; if (size <= large_allocation_threshold) { @@ -1067,7 +1067,7 @@ namespace pugi { struct xml_attribute_struct { - xml_attribute_struct(impl::xml_memory_page* page): name(0), value(0), prev_attribute_c(0), next_attribute(0) + xml_attribute_struct(impl::xml_memory_page* page): name(nullptr), value(nullptr), prev_attribute_c(nullptr), next_attribute(nullptr) { header = PUGI__GETHEADER_IMPL(this, page, 0); } @@ -1083,7 +1083,7 @@ namespace pugi struct xml_node_struct { - xml_node_struct(impl::xml_memory_page* page, xml_node_type type): name(0), value(0), parent(0), first_child(0), prev_sibling_c(0), next_sibling(0), first_attribute(0) + xml_node_struct(impl::xml_memory_page* page, xml_node_type type): name(nullptr), value(nullptr), parent(nullptr), first_child(nullptr), prev_sibling_c(nullptr), next_sibling(nullptr), first_attribute(nullptr) { header = PUGI__GETHEADER_IMPL(this, page, type); } @@ -1114,7 +1114,7 @@ PUGI__NS_BEGIN struct xml_document_struct: public xml_node_struct, public xml_allocator { - xml_document_struct(xml_memory_page* page): xml_node_struct(page, node_document), xml_allocator(page), buffer(0), extra_buffers(0) + xml_document_struct(xml_memory_page* page): xml_node_struct(page, node_document), xml_allocator(page), buffer(nullptr), extra_buffers(nullptr) { #ifdef PUGIXML_COMPACT _hash = &hash; @@ -1151,7 +1151,7 @@ PUGI__NS_BEGIN { xml_memory_page* page; void* memory = alloc.allocate_object(sizeof(xml_attribute_struct), page); - if (!memory) return 0; + if (!memory) return nullptr; return new (memory) xml_attribute_struct(page); } @@ -1160,7 +1160,7 @@ PUGI__NS_BEGIN { xml_memory_page* page; void* memory = alloc.allocate_object(sizeof(xml_node_struct), page); - if (!memory) return 0; + if (!memory) return nullptr; return new (memory) xml_node_struct(page, type); } @@ -1292,9 +1292,9 @@ PUGI__NS_BEGIN else parent->first_child = node->next_sibling; - node->parent = 0; - node->prev_sibling_c = 0; - node->next_sibling = 0; + node->parent = nullptr; + node->prev_sibling_c = nullptr; + node->next_sibling = nullptr; } inline void append_attribute(xml_attribute_struct* attr, xml_node_struct* node) @@ -1368,16 +1368,16 @@ PUGI__NS_BEGIN else node->first_attribute = attr->next_attribute; - attr->prev_attribute_c = 0; - attr->next_attribute = 0; + attr->prev_attribute_c = nullptr; + attr->next_attribute = nullptr; } PUGI__FN_NO_INLINE xml_node_struct* append_new_node(xml_node_struct* node, xml_allocator& alloc, xml_node_type type = node_element) { - if (!alloc.reserve()) return 0; + if (!alloc.reserve()) return nullptr; xml_node_struct* child = allocate_node(alloc, type); - if (!child) return 0; + if (!child) return nullptr; append_node(child, node); @@ -1386,10 +1386,10 @@ PUGI__NS_BEGIN PUGI__FN_NO_INLINE xml_attribute_struct* append_new_attribute(xml_node_struct* node, xml_allocator& alloc) { - if (!alloc.reserve()) return 0; + if (!alloc.reserve()) return nullptr; xml_attribute_struct* attr = allocate_attribute(alloc); - if (!attr) return 0; + if (!attr) return nullptr; append_attribute(attr, node); @@ -1424,7 +1424,7 @@ PUGI__NS_BEGIN struct utf8_counter { - typedef size_t value_type; + using value_type = size_t; static value_type low(value_type result, uint32_t ch) { @@ -1445,7 +1445,7 @@ PUGI__NS_BEGIN struct utf8_writer { - typedef uint8_t* value_type; + using value_type = uint8_t *; static value_type low(value_type result, uint32_t ch) { @@ -1490,7 +1490,7 @@ PUGI__NS_BEGIN struct utf16_counter { - typedef size_t value_type; + using value_type = size_t; static value_type low(value_type result, uint32_t) { @@ -1505,7 +1505,7 @@ PUGI__NS_BEGIN struct utf16_writer { - typedef uint16_t* value_type; + using value_type = uint16_t *; static value_type low(value_type result, uint32_t ch) { @@ -1533,7 +1533,7 @@ PUGI__NS_BEGIN struct utf32_counter { - typedef size_t value_type; + using value_type = size_t; static value_type low(value_type result, uint32_t) { @@ -1548,7 +1548,7 @@ PUGI__NS_BEGIN struct utf32_writer { - typedef uint32_t* value_type; + using value_type = uint32_t *; static value_type low(value_type result, uint32_t ch) { @@ -1574,7 +1574,7 @@ PUGI__NS_BEGIN struct latin1_writer { - typedef uint8_t* value_type; + using value_type = uint8_t *; static value_type low(value_type result, uint32_t ch) { @@ -1595,7 +1595,7 @@ PUGI__NS_BEGIN struct utf8_decoder { - typedef uint8_t type; + using type = uint8_t; template <typename Traits> static inline typename Traits::value_type process(const uint8_t* data, size_t size, typename Traits::value_type result, Traits) { @@ -1662,7 +1662,7 @@ PUGI__NS_BEGIN template <typename opt_swap> struct utf16_decoder { - typedef uint16_t type; + using type = uint16_t; template <typename Traits> static inline typename Traits::value_type process(const uint16_t* data, size_t size, typename Traits::value_type result, Traits) { @@ -1714,7 +1714,7 @@ PUGI__NS_BEGIN template <typename opt_swap> struct utf32_decoder { - typedef uint32_t type; + using type = uint32_t; template <typename Traits> static inline typename Traits::value_type process(const uint32_t* data, size_t size, typename Traits::value_type result, Traits) { @@ -1744,7 +1744,7 @@ PUGI__NS_BEGIN struct latin1_decoder { - typedef uint8_t type; + using type = uint8_t; template <typename Traits> static inline typename Traits::value_type process(const uint8_t* data, size_t size, typename Traits::value_type result, Traits) { @@ -1763,30 +1763,30 @@ PUGI__NS_BEGIN template <> struct wchar_selector<2> { - typedef uint16_t type; - typedef utf16_counter counter; - typedef utf16_writer writer; - typedef utf16_decoder<opt_false> decoder; + using type = uint16_t; + using counter = utf16_counter; + using writer = utf16_writer; + using decoder = utf16_decoder<opt_false>; }; template <> struct wchar_selector<4> { - typedef uint32_t type; - typedef utf32_counter counter; - typedef utf32_writer writer; - typedef utf32_decoder<opt_false> decoder; + using type = uint32_t; + using counter = utf32_counter; + using writer = utf32_writer; + using decoder = utf32_decoder<opt_false>; }; - typedef wchar_selector<sizeof(wchar_t)>::counter wchar_counter; - typedef wchar_selector<sizeof(wchar_t)>::writer wchar_writer; + using wchar_counter = wchar_selector<sizeof(wchar_t)>::counter; + using wchar_writer = wchar_selector<sizeof(wchar_t)>::writer; struct wchar_decoder { - typedef wchar_t type; + using type = wchar_t; template <typename Traits> static inline typename Traits::value_type process(const wchar_t* data, size_t size, typename Traits::value_type result, Traits traits) { - typedef wchar_selector<sizeof(wchar_t)>::decoder decoder; + using decoder = wchar_selector<sizeof(wchar_t)>::decoder; return decoder::process(reinterpret_cast<const typename decoder::type*>(data), size, result, traits); } @@ -2310,7 +2310,7 @@ PUGI__NS_BEGIN char_t* end; size_t size; - gap(): end(0), size(0) + gap(): end(nullptr), size(0) { } @@ -2522,7 +2522,7 @@ PUGI__NS_BEGIN } else if (*s == 0) { - return 0; + return nullptr; } else ++s; } @@ -2550,13 +2550,13 @@ PUGI__NS_BEGIN } else if (*s == 0) { - return 0; + return nullptr; } else ++s; } } - typedef char_t* (*strconv_pcdata_t)(char_t*); + using strconv_pcdata_t = char_t *(*)(char_t *); template <typename opt_trim, typename opt_eol, typename opt_escape> struct strconv_pcdata_impl { @@ -2623,11 +2623,11 @@ PUGI__NS_BEGIN case 5: return strconv_pcdata_impl<opt_true, opt_false, opt_true>::parse; case 6: return strconv_pcdata_impl<opt_true, opt_true, opt_false>::parse; case 7: return strconv_pcdata_impl<opt_true, opt_true, opt_true>::parse; - default: assert(false); return 0; // should not get here + default: assert(false); return nullptr; // should not get here } } - typedef char_t* (*strconv_attribute_t)(char_t*, char_t); + using strconv_attribute_t = char_t *(*)(char_t *, char_t); template <typename opt_escape> struct strconv_attribute_impl { @@ -2677,7 +2677,7 @@ PUGI__NS_BEGIN } else if (!*s) { - return 0; + return nullptr; } else ++s; } @@ -2713,7 +2713,7 @@ PUGI__NS_BEGIN } else if (!*s) { - return 0; + return nullptr; } else ++s; } @@ -2745,7 +2745,7 @@ PUGI__NS_BEGIN } else if (!*s) { - return 0; + return nullptr; } else ++s; } @@ -2771,7 +2771,7 @@ PUGI__NS_BEGIN } else if (!*s) { - return 0; + return nullptr; } else ++s; } @@ -2800,7 +2800,7 @@ PUGI__NS_BEGIN case 13: return strconv_attribute_impl<opt_true>::parse_wnorm; case 14: return strconv_attribute_impl<opt_false>::parse_wnorm; case 15: return strconv_attribute_impl<opt_true>::parse_wnorm; - default: assert(false); return 0; // should not get here + default: assert(false); return nullptr; // should not get here } } @@ -2820,7 +2820,7 @@ PUGI__NS_BEGIN char_t* error_offset; xml_parse_status error_status; - xml_parser(xml_allocator* alloc_): alloc(*alloc_), alloc_state(alloc_), error_offset(0), error_status(status_ok) + xml_parser(xml_allocator* alloc_): alloc(*alloc_), alloc_state(alloc_), error_offset(nullptr), error_status(status_ok) { } @@ -3417,7 +3417,7 @@ PUGI__NS_BEGIN return make_parse_result(PUGI__OPTSET(parse_fragment) ? status_ok : status_no_document_element); // get last child of the root before parsing - xml_node_struct* last_root_child = root->first_child ? root->first_child->prev_sibling_c + 0 : 0; + xml_node_struct* last_root_child = root->first_child ? root->first_child->prev_sibling_c + 0 : nullptr; // create parser on stack xml_parser parser(static_cast<xml_allocator*>(xmldoc)); @@ -3599,8 +3599,8 @@ PUGI__NS_BEGIN class xml_buffered_writer { - xml_buffered_writer(const xml_buffered_writer&); - xml_buffered_writer& operator=(const xml_buffered_writer&); + xml_buffered_writer(const xml_buffered_writer&) = delete; + xml_buffered_writer& operator=(const xml_buffered_writer&) = delete; public: xml_buffered_writer(xml_writer& writer_, xml_encoding user_encoding): writer(writer_), bufsize(0), encoding(get_write_encoding(user_encoding)) @@ -4308,7 +4308,7 @@ PUGI__NS_BEGIN PUGI__FN void node_copy_tree(xml_node_struct* dn, xml_node_struct* sn) { xml_allocator& alloc = get_allocator(dn); - xml_allocator* shared_alloc = (&alloc == &get_allocator(sn)) ? &alloc : 0; + xml_allocator* shared_alloc = (&alloc == &get_allocator(sn)) ? &alloc : nullptr; node_copy_contents(dn, sn, shared_alloc); @@ -4353,7 +4353,7 @@ PUGI__NS_BEGIN PUGI__FN void node_copy_attribute(xml_attribute_struct* da, xml_attribute_struct* sa) { xml_allocator& alloc = get_allocator(da); - xml_allocator* shared_alloc = (&alloc == &get_allocator(sa)) ? &alloc : 0; + xml_allocator* shared_alloc = (&alloc == &get_allocator(sa)) ? &alloc : nullptr; node_copy_string(da->name, da->header, xml_memory_page_name_allocated_mask, sa->name, sa->header, shared_alloc); node_copy_string(da->value, da->header, xml_memory_page_value_allocated_mask, sa->value, sa->header, shared_alloc); @@ -4457,7 +4457,7 @@ PUGI__NS_BEGIN #ifdef PUGIXML_WCHAR_MODE return wcstod(value, 0); #else - return strtod(value, 0); + return strtod(value, nullptr); #endif } @@ -4466,7 +4466,7 @@ PUGI__NS_BEGIN #ifdef PUGIXML_WCHAR_MODE return static_cast<float>(wcstod(value, 0)); #else - return static_cast<float>(strtod(value, 0)); + return static_cast<float>(strtod(value, nullptr)); #endif } @@ -4493,20 +4493,20 @@ PUGI__NS_BEGIN template <typename T> struct make_unsigned; - template <> struct make_unsigned<int> { typedef unsigned int type; }; - template <> struct make_unsigned<unsigned int> { typedef unsigned int type; }; - template <> struct make_unsigned<long> { typedef unsigned long type; }; - template <> struct make_unsigned<unsigned long> { typedef unsigned long type; }; + template <> struct make_unsigned<int> { using type = unsigned int; }; + template <> struct make_unsigned<unsigned int> { using type = unsigned int; }; + template <> struct make_unsigned<long> { using type = unsigned long; }; + template <> struct make_unsigned<unsigned long> { using type = unsigned long; }; #ifdef PUGIXML_HAS_LONG_LONG - template <> struct make_unsigned<long long> { typedef unsigned long long type; }; - template <> struct make_unsigned<unsigned long long> { typedef unsigned long long type; }; + template <> struct make_unsigned<long long> { using type = unsigned long long; }; + template <> struct make_unsigned<unsigned long long> { using type = unsigned long long; }; #endif template <typename T> PUGI__FN char_t* integer_to_string(char_t* begin, char_t* end, T value) { - typedef typename make_unsigned<T>::type U; + using U = typename make_unsigned<T>::type; bool negative = value < 0; @@ -4588,7 +4588,7 @@ PUGI__NS_BEGIN xml_encoding buffer_encoding = impl::get_buffer_encoding(encoding, contents, size); // get private buffer - char_t* buffer = 0; + char_t* buffer = nullptr; size_t length = 0; if (!impl::convert_buffer(buffer, length, buffer_encoding, contents, size, is_mutable)) return impl::make_parse_result(status_out_of_memory); @@ -4630,7 +4630,7 @@ PUGI__NS_BEGIN fseeko64(file, 0, SEEK_SET); #else // if this is a 32-bit OS, long is enough; if this is a unix system, long is 64-bit, which is enough; otherwise we can't do anything anyway. - typedef long length_type; + using length_type = long; fseek(file, 0, SEEK_END); length_type length = ftell(file); @@ -4711,7 +4711,7 @@ PUGI__NS_BEGIN static xml_stream_chunk* create() { void* memory = xml_memory::allocate(sizeof(xml_stream_chunk)); - if (!memory) return 0; + if (!memory) return nullptr; return new (memory) xml_stream_chunk(); } @@ -4729,7 +4729,7 @@ PUGI__NS_BEGIN } } - xml_stream_chunk(): next(0), size(0) + xml_stream_chunk(): next(nullptr), size(0) { } @@ -4741,11 +4741,11 @@ PUGI__NS_BEGIN template <typename T> PUGI__FN xml_parse_status load_stream_data_noseek(std::basic_istream<T>& stream, void** out_buffer, size_t* out_size) { - auto_deleter<xml_stream_chunk<T> > chunks(0, xml_stream_chunk<T>::destroy); + auto_deleter<xml_stream_chunk<T> > chunks(nullptr, xml_stream_chunk<T>::destroy); // read file to a chunk list size_t total = 0; - xml_stream_chunk<T>* last = 0; + xml_stream_chunk<T>* last = nullptr; while (!stream.eof()) { @@ -4831,7 +4831,7 @@ PUGI__NS_BEGIN template <typename T> PUGI__FN xml_parse_result load_stream_impl(xml_document_struct* doc, std::basic_istream<T>& stream, unsigned int options, xml_encoding encoding, char_t** out_buffer) { - void* buffer = 0; + void* buffer = nullptr; size_t size = 0; xml_parse_status status = status_ok; @@ -4871,7 +4871,7 @@ PUGI__NS_BEGIN // allocate resulting string char* result = static_cast<char*>(xml_memory::allocate(size + 1)); - if (!result) return 0; + if (!result) return nullptr; // second pass: convert to utf8 as_utf8_end(result, size, str, length); @@ -4886,7 +4886,7 @@ PUGI__NS_BEGIN { // there is no standard function to open wide paths, so our best bet is to try utf8 path char* path_utf8 = convert_path_heap(path); - if (!path_utf8) return 0; + if (!path_utf8) return nullptr; // convert mode to ASCII (we mirror _wfopen interface) char mode_ascii[4] = {0}; @@ -4919,7 +4919,7 @@ PUGI__NS_BEGIN name_null_sentry(xml_node_struct* node_): node(node_), name(node_->name) { - node->name = 0; + node->name = nullptr; } ~name_null_sentry() @@ -4942,11 +4942,11 @@ namespace pugi } #ifndef PUGIXML_NO_STL - PUGI__FN xml_writer_stream::xml_writer_stream(std::basic_ostream<char, std::char_traits<char> >& stream): narrow_stream(&stream), wide_stream(0) + PUGI__FN xml_writer_stream::xml_writer_stream(std::basic_ostream<char, std::char_traits<char> >& stream): narrow_stream(&stream), wide_stream(nullptr) { } - PUGI__FN xml_writer_stream::xml_writer_stream(std::basic_ostream<wchar_t, std::char_traits<wchar_t> >& stream): narrow_stream(0), wide_stream(&stream) + PUGI__FN xml_writer_stream::xml_writer_stream(std::basic_ostream<wchar_t, std::char_traits<wchar_t> >& stream): narrow_stream(nullptr), wide_stream(&stream) { } @@ -4972,8 +4972,7 @@ namespace pugi } PUGI__FN xml_tree_walker::~xml_tree_walker() - { - } + = default; PUGI__FN int xml_tree_walker::depth() const { @@ -4990,7 +4989,7 @@ namespace pugi return true; } - PUGI__FN xml_attribute::xml_attribute(): _attr(0) + PUGI__FN xml_attribute::xml_attribute(): _attr(nullptr) { } @@ -5004,7 +5003,7 @@ namespace pugi PUGI__FN xml_attribute::operator xml_attribute::unspecified_bool_type() const { - return _attr ? unspecified_bool_xml_attribute : 0; + return _attr ? unspecified_bool_xml_attribute : nullptr; } PUGI__FN bool xml_attribute::operator!() const @@ -5272,7 +5271,7 @@ namespace pugi } #endif - PUGI__FN xml_node::xml_node(): _root(0) + PUGI__FN xml_node::xml_node(): _root(nullptr) { } @@ -5286,7 +5285,7 @@ namespace pugi PUGI__FN xml_node::operator xml_node::unspecified_bool_type() const { - return _root ? unspecified_bool_xml_node : 0; + return _root ? unspecified_bool_xml_node : nullptr; } PUGI__FN bool xml_node::operator!() const @@ -5296,22 +5295,22 @@ namespace pugi PUGI__FN xml_node::iterator xml_node::begin() const { - return iterator(_root ? _root->first_child + 0 : 0, _root); + return iterator(_root ? _root->first_child + 0 : nullptr, _root); } PUGI__FN xml_node::iterator xml_node::end() const { - return iterator(0, _root); + return iterator(nullptr, _root); } PUGI__FN xml_node::attribute_iterator xml_node::attributes_begin() const { - return attribute_iterator(_root ? _root->first_attribute + 0 : 0, _root); + return attribute_iterator(_root ? _root->first_attribute + 0 : nullptr, _root); } PUGI__FN xml_node::attribute_iterator xml_node::attributes_end() const { - return attribute_iterator(0, _root); + return attribute_iterator(nullptr, _root); } PUGI__FN xml_object_range<xml_node_iterator> xml_node::children() const @@ -5321,7 +5320,7 @@ namespace pugi PUGI__FN xml_object_range<xml_named_node_iterator> xml_node::children(const char_t* name_) const { - return xml_object_range<xml_named_node_iterator>(xml_named_node_iterator(child(name_)._root, _root, name_), xml_named_node_iterator(0, _root, name_)); + return xml_object_range<xml_named_node_iterator>(xml_named_node_iterator(child(name_)._root, _root, name_), xml_named_node_iterator(nullptr, _root, name_)); } PUGI__FN xml_object_range<xml_attribute_iterator> xml_node::attributes() const @@ -5974,14 +5973,14 @@ namespace pugi doc->header |= impl::xml_memory_page_contents_shared_mask; // get extra buffer element (we'll store the document fragment buffer there so that we can deallocate it later) - impl::xml_memory_page* page = 0; + impl::xml_memory_page* page = nullptr; impl::xml_extra_buffer* extra = static_cast<impl::xml_extra_buffer*>(doc->allocate_memory(sizeof(impl::xml_extra_buffer), page)); (void)page; if (!extra) return impl::make_parse_result(status_out_of_memory); // add extra buffer to the list - extra->buffer = 0; + extra->buffer = nullptr; extra->next = doc->extra_buffers; doc->extra_buffers = extra; @@ -6244,7 +6243,7 @@ namespace pugi if (impl::is_text_node(node)) return node; - return 0; + return nullptr; } PUGI__FN xml_node_struct* xml_text::_data_new() @@ -6255,7 +6254,7 @@ namespace pugi return xml_node(_root).append_child(node_pcdata).internal_object(); } - PUGI__FN xml_text::xml_text(): _root(0) + PUGI__FN xml_text::xml_text(): _root(nullptr) { } @@ -6265,7 +6264,7 @@ namespace pugi PUGI__FN xml_text::operator xml_text::unspecified_bool_type() const { - return _data() ? unspecified_bool_xml_text : 0; + return _data() ? unspecified_bool_xml_text : nullptr; } PUGI__FN bool xml_text::operator!() const @@ -6275,7 +6274,7 @@ namespace pugi PUGI__FN bool xml_text::empty() const { - return _data() == 0; + return _data() == nullptr; } PUGI__FN const char_t* xml_text::get() const @@ -6495,8 +6494,7 @@ namespace pugi #endif PUGI__FN xml_node_iterator::xml_node_iterator() - { - } + = default; PUGI__FN xml_node_iterator::xml_node_iterator(const xml_node& node): _wrap(node), _parent(node.parent()) { @@ -6556,8 +6554,7 @@ namespace pugi } PUGI__FN xml_attribute_iterator::xml_attribute_iterator() - { - } + = default; PUGI__FN xml_attribute_iterator::xml_attribute_iterator(const xml_attribute& attr, const xml_node& parent): _wrap(attr), _parent(parent) { @@ -6616,7 +6613,7 @@ namespace pugi return temp; } - PUGI__FN xml_named_node_iterator::xml_named_node_iterator(): _name(0) + PUGI__FN xml_named_node_iterator::xml_named_node_iterator(): _name(nullptr) { } @@ -6726,7 +6723,7 @@ namespace pugi } } - PUGI__FN xml_document::xml_document(): _buffer(0) + PUGI__FN xml_document::xml_document(): _buffer(nullptr) { create(); } @@ -6795,7 +6792,7 @@ namespace pugi if (_buffer) { impl::xml_memory::deallocate(_buffer); - _buffer = 0; + _buffer = nullptr; } // destroy extra buffers (note: no need to destroy linked list nodes, they're allocated using document allocator) @@ -6823,7 +6820,7 @@ namespace pugi static_cast<impl::xml_document_struct*>(_root)->hash.clear(); #endif - _root = 0; + _root = nullptr; } #ifndef PUGIXML_NO_STL @@ -6864,7 +6861,7 @@ namespace pugi reset(); using impl::auto_deleter; // MSVC7 workaround - typedef int (*fclose_t)(FILE*); // BCC5 workaround + using fclose_t = int (*)(FILE *); // BCC5 workaround auto_deleter<FILE, fclose_t> file(fopen(path_, "rb"), fclose); return impl::load_file_impl(static_cast<impl::xml_document_struct*>(_root), file.data, options, encoding, &_buffer); @@ -6875,7 +6872,7 @@ namespace pugi reset(); using impl::auto_deleter; // MSVC7 workaround - typedef int (*fclose_t)(FILE*); // BCC5 workaround + using fclose_t = int (*)(FILE *); // BCC5 workaround auto_deleter<FILE, fclose_t> file(impl::open_file_wide(path_, L"rb"), fclose); return impl::load_file_impl(static_cast<impl::xml_document_struct*>(_root), file.data, options, encoding, &_buffer); @@ -6949,7 +6946,7 @@ namespace pugi PUGI__FN bool xml_document::save_file(const char* path_, const char_t* indent, unsigned int flags, xml_encoding encoding) const { using impl::auto_deleter; // MSVC7 workaround - typedef int (*fclose_t)(FILE*); // BCC5 workaround + using fclose_t = int (*)(FILE *); // BCC5 workaround auto_deleter<FILE, fclose_t> file(fopen(path_, (flags & format_save_file_text) ? "w" : "wb"), fclose); return impl::save_file_impl(*this, file.data, indent, flags, encoding); @@ -6958,7 +6955,7 @@ namespace pugi PUGI__FN bool xml_document::save_file(const wchar_t* path_, const char_t* indent, unsigned int flags, xml_encoding encoding) const { using impl::auto_deleter; // MSVC7 workaround - typedef int (*fclose_t)(FILE*); // BCC5 workaround + using fclose_t = int (*)(FILE *); // BCC5 workaround auto_deleter<FILE, fclose_t> file(impl::open_file_wide(path_, (flags & format_save_file_text) ? L"w" : L"wb"), fclose); return impl::save_file_impl(*this, file.data, indent, flags, encoding); @@ -7350,7 +7347,7 @@ PUGI__NS_BEGIN size_t block_size = block_capacity + offsetof(xpath_memory_block, data); xpath_memory_block* block = static_cast<xpath_memory_block*>(xml_memory::allocate(block_size)); - if (!block) return 0; + if (!block) return nullptr; block->next = _root; block->capacity = block_capacity; @@ -7386,7 +7383,7 @@ PUGI__NS_BEGIN new_size = (new_size + xpath_memory_block_alignment - 1) & ~(xpath_memory_block_alignment - 1); // we can only reallocate the last object - assert(ptr == 0 || static_cast<char*>(ptr) + old_size == &_root->data[0] + _root_size); + assert(ptr == nullptr || static_cast<char*>(ptr) + old_size == &_root->data[0] + _root_size); // adjust root size so that we have not allocated the object at all bool only_object = (_root_size == old_size); @@ -7493,7 +7490,7 @@ PUGI__NS_BEGIN xpath_stack_data(): result(blocks + 0), temp(blocks + 1) { - blocks[0].next = blocks[1].next = 0; + blocks[0].next = blocks[1].next = nullptr; blocks[0].capacity = blocks[1].capacity = sizeof(blocks[0].data); stack.result = &result; @@ -7579,7 +7576,7 @@ PUGI__NS_BEGIN size_t result_length = target_length + source_length; // allocate new buffer - char_t* result = static_cast<char_t*>(alloc->reallocate(_uses_heap ? const_cast<char_t*>(_buffer) : 0, (target_length + 1) * sizeof(char_t), (result_length + 1) * sizeof(char_t))); + char_t* result = static_cast<char_t*>(alloc->reallocate(_uses_heap ? const_cast<char_t*>(_buffer) : nullptr, (target_length + 1) * sizeof(char_t), (result_length + 1) * sizeof(char_t))); assert(result); // append first string to the new buffer in case there was no reallocation @@ -7820,7 +7817,7 @@ PUGI__NS_BEGIN if (node->value && (node->header & impl::xml_memory_page_value_allocated_or_shared_mask) == 0) return node->value; } - return 0; + return nullptr; } xml_attribute_struct* attr = xnode.attribute().internal_object(); @@ -7833,10 +7830,10 @@ PUGI__NS_BEGIN if ((attr->header & impl::xml_memory_page_value_allocated_or_shared_mask) == 0) return attr->value; } - return 0; + return nullptr; } - return 0; + return nullptr; } struct document_order_comparator @@ -7906,7 +7903,7 @@ PUGI__NS_BEGIN { #if defined(__STDC_IEC_559__) || ((FLT_RADIX - 0 == 2) && (FLT_MAX_EXP - 0 == 128) && (FLT_MANT_DIG - 0 == 24)) PUGI__STATIC_ASSERT(sizeof(float) == sizeof(uint32_t)); - typedef uint32_t UI; // BCC5 workaround + using UI = uint32_t; // BCC5 workaround union { float f; UI i; } u; u.i = 0x7fc00000; return u.f; @@ -7958,7 +7955,7 @@ PUGI__NS_BEGIN if (v == 0) return PUGIXML_TEXT("0"); if (v != v) return PUGIXML_TEXT("NaN"); if (v * 2 == v) return value > 0 ? PUGIXML_TEXT("Infinity") : PUGIXML_TEXT("-Infinity"); - return 0; + return nullptr; #endif } @@ -8127,7 +8124,7 @@ PUGI__NS_BEGIN #ifdef PUGIXML_WCHAR_MODE return wcstod(string, 0); #else - return strtod(string, 0); + return strtod(string, nullptr); #endif } @@ -8189,7 +8186,7 @@ PUGI__NS_BEGIN { const char_t* pos = find_char(name, ':'); - prefix = pos ? name : 0; + prefix = pos ? name : nullptr; prefix_length = pos ? static_cast<size_t>(pos - name) : 0; } @@ -8307,7 +8304,7 @@ PUGI__NS_BEGIN unsigned int tc = static_cast<unsigned int>(*to); if (fc >= 128 || tc >= 128) - return 0; + return nullptr; // code=128 means "skip character" if (!table[fc]) @@ -8388,7 +8385,7 @@ PUGI__NS_BEGIN struct xpath_variable_string: xpath_variable { - xpath_variable_string(): xpath_variable(xpath_type_string), value(0) + xpath_variable_string(): xpath_variable(xpath_type_string), value(nullptr) { } @@ -8435,11 +8432,11 @@ PUGI__NS_BEGIN template <typename T> PUGI__FN T* new_xpath_variable(const char_t* name) { size_t length = strlength(name); - if (length == 0) return 0; // empty variable names are invalid + if (length == 0) return nullptr; // empty variable names are invalid // $$ we can't use offsetof(T, name) because T is non-POD, so we just allocate additional length characters void* memory = xml_memory::allocate(sizeof(T) + length * sizeof(char_t)); - if (!memory) return 0; + if (!memory) return nullptr; T* result = new (memory) T(); @@ -8465,7 +8462,7 @@ PUGI__NS_BEGIN return new_xpath_variable<xpath_variable_boolean>(name); default: - return 0; + return nullptr; } } @@ -8618,7 +8615,7 @@ PUGI__NS_BEGIN xpath_node* _eos; public: - xpath_node_set_raw(): _type(xpath_node_set::type_unsorted), _begin(0), _end(0), _eos(0) + xpath_node_set_raw(): _type(xpath_node_set::type_unsorted), _begin(nullptr), _end(nullptr), _eos(nullptr) { } @@ -8780,7 +8777,7 @@ PUGI__NS_BEGIN const char_t* begin; const char_t* end; - xpath_lexer_string(): begin(0), end(0) + xpath_lexer_string(): begin(nullptr), end(nullptr) { } @@ -9247,8 +9244,8 @@ PUGI__NS_BEGIN const unsigned char* table; } _data; - xpath_ast_node(const xpath_ast_node&); - xpath_ast_node& operator=(const xpath_ast_node&); + xpath_ast_node(const xpath_ast_node&) = delete; + xpath_ast_node& operator=(const xpath_ast_node&) = delete; template <class Comp> static bool compare_eq(xpath_ast_node* lhs, xpath_ast_node* rhs, const xpath_context& c, const xpath_stack& stack, const Comp& comp) { @@ -9277,12 +9274,12 @@ PUGI__NS_BEGIN xpath_node_set_raw ls = lhs->eval_node_set(c, stack, nodeset_eval_all); xpath_node_set_raw rs = rhs->eval_node_set(c, stack, nodeset_eval_all); - for (const xpath_node* li = ls.begin(); li != ls.end(); ++li) - for (const xpath_node* ri = rs.begin(); ri != rs.end(); ++ri) + for (auto l : ls) + for (auto r : rs) { xpath_allocator_capture cri(stack.result); - if (comp(string_value(*li, stack.result), string_value(*ri, stack.result))) + if (comp(string_value(l, stack.result), string_value(r, stack.result))) return true; } @@ -9305,11 +9302,11 @@ PUGI__NS_BEGIN double l = lhs->eval_number(c, stack); xpath_node_set_raw rs = rhs->eval_node_set(c, stack, nodeset_eval_all); - for (const xpath_node* ri = rs.begin(); ri != rs.end(); ++ri) + for (auto r : rs) { xpath_allocator_capture cri(stack.result); - if (comp(l, convert_string_to_number(string_value(*ri, stack.result).c_str()))) + if (comp(l, convert_string_to_number(string_value(r, stack.result).c_str()))) return true; } @@ -9322,11 +9319,11 @@ PUGI__NS_BEGIN xpath_string l = lhs->eval_string(c, stack); xpath_node_set_raw rs = rhs->eval_node_set(c, stack, nodeset_eval_all); - for (const xpath_node* ri = rs.begin(); ri != rs.end(); ++ri) + for (auto r : rs) { xpath_allocator_capture cri(stack.result); - if (comp(l, string_value(*ri, stack.result))) + if (comp(l, string_value(r, stack.result))) return true; } @@ -9356,17 +9353,17 @@ PUGI__NS_BEGIN xpath_node_set_raw ls = lhs->eval_node_set(c, stack, nodeset_eval_all); xpath_node_set_raw rs = rhs->eval_node_set(c, stack, nodeset_eval_all); - for (const xpath_node* li = ls.begin(); li != ls.end(); ++li) + for (auto li : ls) { xpath_allocator_capture cri(stack.result); - double l = convert_string_to_number(string_value(*li, stack.result).c_str()); + double l = convert_string_to_number(string_value(li, stack.result).c_str()); - for (const xpath_node* ri = rs.begin(); ri != rs.end(); ++ri) + for (auto r : rs) { xpath_allocator_capture crii(stack.result); - if (comp(l, convert_string_to_number(string_value(*ri, stack.result).c_str()))) + if (comp(l, convert_string_to_number(string_value(r, stack.result).c_str()))) return true; } } @@ -9380,11 +9377,11 @@ PUGI__NS_BEGIN double l = lhs->eval_number(c, stack); xpath_node_set_raw rs = rhs->eval_node_set(c, stack, nodeset_eval_all); - for (const xpath_node* ri = rs.begin(); ri != rs.end(); ++ri) + for (auto r : rs) { xpath_allocator_capture cri(stack.result); - if (comp(l, convert_string_to_number(string_value(*ri, stack.result).c_str()))) + if (comp(l, convert_string_to_number(string_value(r, stack.result).c_str()))) return true; } @@ -9397,11 +9394,11 @@ PUGI__NS_BEGIN xpath_node_set_raw ls = lhs->eval_node_set(c, stack, nodeset_eval_all); double r = rhs->eval_number(c, stack); - for (const xpath_node* li = ls.begin(); li != ls.end(); ++li) + for (auto l : ls) { xpath_allocator_capture cri(stack.result); - if (comp(convert_string_to_number(string_value(*li, stack.result).c_str()), r)) + if (comp(convert_string_to_number(string_value(l, stack.result).c_str()), r)) return true; } @@ -9939,14 +9936,14 @@ PUGI__NS_BEGIN // self axis preserves the original order if (axis == axis_self) ns.set_type(s.type()); - for (const xpath_node* it = s.begin(); it != s.end(); ++it) + for (auto it : s) { size_t size = ns.size(); // in general, all axes generate elements in a particular order, but there is no order guarantee if axis is applied to two nodes if (axis != axis_self && size != 0) ns.set_type(xpath_node_set::type_unsorted); - step_fill(ns, *it, stack.result, once, v); + step_fill(ns, it, stack.result, once, v); if (_right) apply_predicates(ns, size, stack, eval); } } @@ -9966,40 +9963,40 @@ PUGI__NS_BEGIN public: xpath_ast_node(ast_type_t type, xpath_value_type rettype_, const char_t* value): - _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(0), _right(0), _next(0) + _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(nullptr), _right(nullptr), _next(nullptr) { assert(type == ast_string_constant); _data.string = value; } xpath_ast_node(ast_type_t type, xpath_value_type rettype_, double value): - _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(0), _right(0), _next(0) + _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(nullptr), _right(nullptr), _next(nullptr) { assert(type == ast_number_constant); _data.number = value; } xpath_ast_node(ast_type_t type, xpath_value_type rettype_, xpath_variable* value): - _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(0), _right(0), _next(0) + _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(nullptr), _right(nullptr), _next(nullptr) { assert(type == ast_variable); _data.variable = value; } - xpath_ast_node(ast_type_t type, xpath_value_type rettype_, xpath_ast_node* left = 0, xpath_ast_node* right = 0): - _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(left), _right(right), _next(0) + xpath_ast_node(ast_type_t type, xpath_value_type rettype_, xpath_ast_node* left = nullptr, xpath_ast_node* right = nullptr): + _type(static_cast<char>(type)), _rettype(static_cast<char>(rettype_)), _axis(0), _test(0), _left(left), _right(right), _next(nullptr) { } xpath_ast_node(ast_type_t type, xpath_ast_node* left, axis_t axis, nodetest_t test, const char_t* contents): - _type(static_cast<char>(type)), _rettype(xpath_type_node_set), _axis(static_cast<char>(axis)), _test(static_cast<char>(test)), _left(left), _right(0), _next(0) + _type(static_cast<char>(type)), _rettype(xpath_type_node_set), _axis(static_cast<char>(axis)), _test(static_cast<char>(test)), _left(left), _right(nullptr), _next(nullptr) { assert(type == ast_step); _data.nodetest = contents; } xpath_ast_node(ast_type_t type, xpath_ast_node* left, xpath_ast_node* right, predicate_t test): - _type(static_cast<char>(type)), _rettype(xpath_type_node_set), _axis(0), _test(static_cast<char>(test)), _left(left), _right(right), _next(0) + _type(static_cast<char>(type)), _rettype(xpath_type_node_set), _axis(0), _test(static_cast<char>(test)), _left(left), _right(right), _next(nullptr) { assert(type == ast_filter || type == ast_predicate); } @@ -10059,7 +10056,7 @@ PUGI__NS_BEGIN xpath_string lr = _left->eval_string(c, stack); xpath_string rr = _right->eval_string(c, stack); - return find_substring(lr.c_str(), rr.c_str()) != 0; + return find_substring(lr.c_str(), rr.c_str()) != nullptr; } case ast_func_boolean: @@ -10222,11 +10219,11 @@ PUGI__NS_BEGIN xpath_node_set_raw ns = _left->eval_node_set(c, stack, nodeset_eval_all); - for (const xpath_node* it = ns.begin(); it != ns.end(); ++it) + for (auto n : ns) { xpath_allocator_capture cri(stack.result); - r += convert_string_to_number(string_value(*it, stack.result).c_str()); + r += convert_string_to_number(string_value(n, stack.result).c_str()); } return r; @@ -10881,7 +10878,7 @@ PUGI__NS_BEGIN return c; } - else return 0; + else return nullptr; } xpath_ast_node* parse_function_helper(ast_type_t type0, ast_type_t type1, size_t argc, xpath_ast_node* args[2]) @@ -11003,7 +11000,7 @@ PUGI__NS_BEGIN throw_error("Unrecognized function or wrong parameter count"); - return 0; + return nullptr; } axis_t parse_axis_name(const xpath_lexer_string& name, bool& specified) @@ -11121,7 +11118,7 @@ PUGI__NS_BEGIN if (!_variables) throw_error("Unknown variable: variable set is not provided"); - xpath_variable* var = 0; + xpath_variable* var = nullptr; if (!get_variable_scratch(_scratch, _variables, name.begin, name.end, &var)) throw_error_oom(); @@ -11172,13 +11169,13 @@ PUGI__NS_BEGIN case lex_string: { - xpath_ast_node* args[2] = {0}; + xpath_ast_node* args[2] = {nullptr}; size_t argc = 0; xpath_lexer_string function = _lexer.contents(); _lexer.next(); - xpath_ast_node* last_arg = 0; + xpath_ast_node* last_arg = nullptr; if (_lexer.current() != lex_open_brace) throw_error("Unrecognized function call"); @@ -11210,7 +11207,7 @@ PUGI__NS_BEGIN default: throw_error("Unrecognizable primary expression"); - return 0; + return nullptr; } } @@ -11264,13 +11261,13 @@ PUGI__NS_BEGIN { _lexer.next(); - return new (alloc_node()) xpath_ast_node(ast_step, set, axis_self, nodetest_type_node, 0); + return new (alloc_node()) xpath_ast_node(ast_step, set, axis_self, nodetest_type_node, nullptr); } else if (_lexer.current() == lex_double_dot) { _lexer.next(); - return new (alloc_node()) xpath_ast_node(ast_step, set, axis_parent, nodetest_type_node, 0); + return new (alloc_node()) xpath_ast_node(ast_step, set, axis_parent, nodetest_type_node, nullptr); } nodetest_t nt_type = nodetest_none; @@ -11365,7 +11362,7 @@ PUGI__NS_BEGIN xpath_ast_node* n = new (alloc_node()) xpath_ast_node(ast_step, set, axis, nt_type, alloc_string(nt_name)); - xpath_ast_node* last = 0; + xpath_ast_node* last = nullptr; while (_lexer.current() == lex_open_square_brace) { @@ -11373,7 +11370,7 @@ PUGI__NS_BEGIN xpath_ast_node* expr = parse_expression(); - xpath_ast_node* pred = new (alloc_node()) xpath_ast_node(ast_predicate, 0, expr, predicate_default); + xpath_ast_node* pred = new (alloc_node()) xpath_ast_node(ast_predicate, nullptr, expr, predicate_default); if (_lexer.current() != lex_close_square_brace) throw_error("Unmatched square brace"); @@ -11399,7 +11396,7 @@ PUGI__NS_BEGIN _lexer.next(); if (l == lex_double_slash) - n = new (alloc_node()) xpath_ast_node(ast_step, n, axis_descendant_or_self, nodetest_type_node, 0); + n = new (alloc_node()) xpath_ast_node(ast_step, n, axis_descendant_or_self, nodetest_type_node, nullptr); n = parse_step(n); } @@ -11430,13 +11427,13 @@ PUGI__NS_BEGIN _lexer.next(); xpath_ast_node* n = new (alloc_node()) xpath_ast_node(ast_step_root, xpath_type_node_set); - n = new (alloc_node()) xpath_ast_node(ast_step, n, axis_descendant_or_self, nodetest_type_node, 0); + n = new (alloc_node()) xpath_ast_node(ast_step, n, axis_descendant_or_self, nodetest_type_node, nullptr); return parse_relative_location_path(n); } // else clause moved outside of if because of bogus warning 'control may reach end of non-void function being inlined' in gcc 4.0.1 - return parse_relative_location_path(0); + return parse_relative_location_path(nullptr); } // PathExpr ::= LocationPath @@ -11482,7 +11479,7 @@ PUGI__NS_BEGIN { if (n->rettype() != xpath_type_node_set) throw_error("Step has to be applied to node set"); - n = new (alloc_node()) xpath_ast_node(ast_step, n, axis_descendant_or_self, nodetest_type_node, 0); + n = new (alloc_node()) xpath_ast_node(ast_step, n, axis_descendant_or_self, nodetest_type_node, nullptr); } // select from location path @@ -11659,7 +11656,7 @@ PUGI__NS_BEGIN static xpath_query_impl* create() { void* memory = xml_memory::allocate(sizeof(xpath_query_impl)); - if (!memory) return 0; + if (!memory) return nullptr; return new (memory) xpath_query_impl(); } @@ -11673,9 +11670,9 @@ PUGI__NS_BEGIN xml_memory::deallocate(impl); } - xpath_query_impl(): root(0), alloc(&block) + xpath_query_impl(): root(nullptr), alloc(&block) { - block.next = 0; + block.next = nullptr; block.capacity = sizeof(block.data); } @@ -11699,7 +11696,7 @@ PUGI__NS_BEGIN PUGI__FN impl::xpath_ast_node* evaluate_node_set_prepare(xpath_query_impl* impl) { - if (!impl) return 0; + if (!impl) return nullptr; if (impl->root->rettype() != xpath_type_node_set) { @@ -11737,8 +11734,7 @@ namespace pugi #endif PUGI__FN xpath_node::xpath_node() - { - } + = default; PUGI__FN xpath_node::xpath_node(const xml_node& node_): _node(node_) { @@ -11769,7 +11765,7 @@ namespace pugi PUGI__FN xpath_node::operator xpath_node::unspecified_bool_type() const { - return (_node || _attribute) ? unspecified_bool_xpath_node : 0; + return (_node || _attribute) ? unspecified_bool_xpath_node : nullptr; } PUGI__FN bool xpath_node::operator!() const @@ -11952,7 +11948,7 @@ namespace pugi PUGI__FN xpath_parse_result::operator bool() const { - return error == 0; + return error == nullptr; } PUGI__FN const char* xpath_parse_result::description() const @@ -11960,7 +11956,7 @@ namespace pugi return error ? error : "No error"; } - PUGI__FN xpath_variable::xpath_variable(xpath_value_type type_): _type(type_), _next(0) + PUGI__FN xpath_variable::xpath_variable(xpath_value_type type_): _type(type_), _next(nullptr) { } @@ -11982,7 +11978,7 @@ namespace pugi default: assert(false && "Invalid variable type"); - return 0; + return nullptr; } } @@ -12003,7 +11999,7 @@ namespace pugi PUGI__FN const char_t* xpath_variable::get_string() const { - const char_t* value = (_type == xpath_type_string) ? static_cast<const impl::xpath_variable_string*>(this)->value : 0; + const char_t* value = (_type == xpath_type_string) ? static_cast<const impl::xpath_variable_string*>(this)->value : nullptr; return value ? value : PUGIXML_TEXT(""); } @@ -12059,20 +12055,20 @@ namespace pugi PUGI__FN xpath_variable_set::xpath_variable_set() { - for (size_t i = 0; i < sizeof(_data) / sizeof(_data[0]); ++i) - _data[i] = 0; + for (auto & i : _data) + i = nullptr; } PUGI__FN xpath_variable_set::~xpath_variable_set() { - for (size_t i = 0; i < sizeof(_data) / sizeof(_data[0]); ++i) - _destroy(_data[i]); + for (auto & i : _data) + _destroy(i); } PUGI__FN xpath_variable_set::xpath_variable_set(const xpath_variable_set& rhs) { - for (size_t i = 0; i < sizeof(_data) / sizeof(_data[0]); ++i) - _data[i] = 0; + for (auto & i : _data) + i = nullptr; _assign(rhs); } @@ -12092,7 +12088,7 @@ namespace pugi for (size_t i = 0; i < sizeof(_data) / sizeof(_data[0]); ++i) { _data[i] = rhs._data[i]; - rhs._data[i] = 0; + rhs._data[i] = nullptr; } } @@ -12103,7 +12099,7 @@ namespace pugi _destroy(_data[i]); _data[i] = rhs._data[i]; - rhs._data[i] = 0; + rhs._data[i] = nullptr; } return *this; @@ -12142,12 +12138,12 @@ namespace pugi if (impl::strequal(var->name(), name)) return var; - return 0; + return nullptr; } PUGI__FN bool xpath_variable_set::_clone(xpath_variable* var, xpath_variable** out_result) { - xpath_variable* last = 0; + xpath_variable* last = nullptr; while (var) { @@ -12192,7 +12188,7 @@ namespace pugi // look for existing variable for (xpath_variable* var = _data[hash]; var; var = var->_next) if (impl::strequal(var->name(), name)) - return var->type() == type ? var : 0; + return var->type() == type ? var : nullptr; // add new variable xpath_variable* result = impl::new_xpath_variable(type, name); @@ -12241,7 +12237,7 @@ namespace pugi return _find(name); } - PUGI__FN xpath_query::xpath_query(const char_t* query, xpath_variable_set* variables): _impl(0) + PUGI__FN xpath_query::xpath_query(const char_t* query, xpath_variable_set* variables): _impl(nullptr) { impl::xpath_query_impl* qimpl = impl::xpath_query_impl::create(); @@ -12265,12 +12261,12 @@ namespace pugi qimpl->root->optimize(&qimpl->alloc); _impl = impl.release(); - _result.error = 0; + _result.error = nullptr; } } } - PUGI__FN xpath_query::xpath_query(): _impl(0) + PUGI__FN xpath_query::xpath_query(): _impl(nullptr) { } @@ -12285,7 +12281,7 @@ namespace pugi { _impl = rhs._impl; _result = rhs._result; - rhs._impl = 0; + rhs._impl = nullptr; rhs._result = xpath_parse_result(); } @@ -12298,7 +12294,7 @@ namespace pugi _impl = rhs._impl; _result = rhs._result; - rhs._impl = 0; + rhs._impl = nullptr; rhs._result = xpath_parse_result(); return *this; @@ -12416,7 +12412,7 @@ namespace pugi PUGI__FN xpath_query::operator xpath_query::unspecified_bool_type() const { - return _impl ? unspecified_bool_xpath_query : 0; + return _impl ? unspecified_bool_xpath_query : nullptr; } PUGI__FN bool xpath_query::operator!() const diff --git a/VirtualRobot/Import/MeshImport/STLReader.cpp b/VirtualRobot/Import/MeshImport/STLReader.cpp index 47e0120203e3e2dbe04a453cba7f425fb456450c..39021a47214b65fffe491e3216e5156cf4236444 100644 --- a/VirtualRobot/Import/MeshImport/STLReader.cpp +++ b/VirtualRobot/Import/MeshImport/STLReader.cpp @@ -41,7 +41,7 @@ // STL #include <map> -#include <float.h> +#include <cfloat> #include <fstream> #include <stdexcept> #include <string> diff --git a/VirtualRobot/Import/SimoxCOLLADAFactory.cpp b/VirtualRobot/Import/SimoxCOLLADAFactory.cpp index 83b541557df9d08d06886fcb8c8ec3b8ac0fb5d0..49329820e2effae86c54e264bfd8104c72cf7c86 100644 --- a/VirtualRobot/Import/SimoxCOLLADAFactory.cpp +++ b/VirtualRobot/Import/SimoxCOLLADAFactory.cpp @@ -8,13 +8,11 @@ namespace VirtualRobot { SimoxCOLLADAFactory::SimoxCOLLADAFactory() - { - } + = default; SimoxCOLLADAFactory::~SimoxCOLLADAFactory() - { - } + = default; RobotPtr SimoxCOLLADAFactory::loadFromFile(const std::string& filename, RobotIO::RobotDescription /*loadMode*/) diff --git a/VirtualRobot/Import/SimoxXMLFactory.cpp b/VirtualRobot/Import/SimoxXMLFactory.cpp index e70a8fca71a15e5db1bd6faf6971f2ff62f6019c..15028dfb933c6e3256e4c60153048ad0cfbb055b 100644 --- a/VirtualRobot/Import/SimoxXMLFactory.cpp +++ b/VirtualRobot/Import/SimoxXMLFactory.cpp @@ -9,13 +9,11 @@ namespace VirtualRobot { SimoxXMLFactory::SimoxXMLFactory() - { - } + = default; SimoxXMLFactory::~SimoxXMLFactory() - { - } + = default; RobotPtr SimoxXMLFactory::loadFromFile(const std::string& filename, RobotIO::RobotDescription loadMode) diff --git a/VirtualRobot/KinematicChain.cpp b/VirtualRobot/KinematicChain.cpp index 01004f559b4dfa3e6b0ddde1ef202609d89d3d17..c933d7516b14eeb077d18b598d60c636b8836261 100644 --- a/VirtualRobot/KinematicChain.cpp +++ b/VirtualRobot/KinematicChain.cpp @@ -45,9 +45,7 @@ namespace VirtualRobot KinematicChain::~KinematicChain() - { - - } + = default; diff --git a/VirtualRobot/LinkedCoordinate.cpp b/VirtualRobot/LinkedCoordinate.cpp index e0e1082a58007da18ed805c9e306b7f6cbb9bdfa..7e66fb4d56808544c75a891c079536bc37860f78 100644 --- a/VirtualRobot/LinkedCoordinate.cpp +++ b/VirtualRobot/LinkedCoordinate.cpp @@ -16,12 +16,7 @@ LinkedCoordinate::LinkedCoordinate(const LinkedCoordinate& other) } LinkedCoordinate& LinkedCoordinate::operator=(const LinkedCoordinate& other) -{ - robot = other.robot; - pose = other.pose; - frame = other.frame; - return *this; -} += default; LinkedCoordinate::~LinkedCoordinate() { diff --git a/VirtualRobot/ManipulationObject.cpp b/VirtualRobot/ManipulationObject.cpp index 6792536ada5526d2c06cf0fe7b9764a4179a0368..813efeb31923353499e0bdb999ac4158edf6bcf3 100644 --- a/VirtualRobot/ManipulationObject.cpp +++ b/VirtualRobot/ManipulationObject.cpp @@ -14,8 +14,7 @@ namespace VirtualRobot } ManipulationObject::~ManipulationObject() - { - } + = default; void ManipulationObject::print(bool printDecoration) { @@ -71,8 +70,8 @@ namespace VirtualRobot { VR_ASSERT_MESSAGE(graspSet, "NULL data"); - for (size_t i = 0; i < graspSets.size(); i++) - if (graspSets[i] == graspSet) + for (const auto & i : graspSets) + if (i == graspSet) { return true; } @@ -82,8 +81,8 @@ namespace VirtualRobot bool ManipulationObject::hasGraspSet(const std::string& robotType, const std::string& eef) { - for (size_t i = 0; i < graspSets.size(); i++) - if (graspSets[i]->getRobotType() == robotType && graspSets[i]->getEndEffector() == eef) + for (auto & graspSet : graspSets) + if (graspSet->getRobotType() == robotType && graspSet->getEndEffector() == eef) { return true; } @@ -102,10 +101,10 @@ namespace VirtualRobot VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet(const std::string& robotType, const std::string& eefName) { - for (size_t i = 0; i < graspSets.size(); i++) - if (graspSets[i]->getRobotType() == robotType && graspSets[i]->getEndEffector() == eefName) + for (auto & graspSet : graspSets) + if (graspSet->getRobotType() == robotType && graspSet->getEndEffector() == eefName) { - return graspSets[i]; + return graspSet; } return GraspSetPtr(); @@ -113,10 +112,10 @@ namespace VirtualRobot VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet(const std::string& name) { - for (size_t i = 0; i < graspSets.size(); i++) - if (graspSets[i]->getName() == name) + for (auto & graspSet : graspSets) + if (graspSet->getName() == name) { - return graspSets[i]; + return graspSet; } return GraspSetPtr(); @@ -166,9 +165,9 @@ namespace VirtualRobot ss << getSceneObjectXMLString(basePath, tabs + 1); - for (size_t i = 0; i < graspSets.size(); i++) + for (auto & graspSet : graspSets) { - ss << graspSets[i]->getXMLString(tabs + 1) << "\n"; + ss << graspSet->getXMLString(tabs + 1) << "\n"; } } @@ -208,9 +207,9 @@ namespace VirtualRobot result->setGlobalPose(getGlobalPose()); - for (size_t i = 0; i < graspSets.size(); i++) + for (const auto & graspSet : graspSets) { - result->addGraspSet(graspSets[i]->clone()); + result->addGraspSet(graspSet->clone()); } return result; diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp index bfc0fb1a3ad10475a207975f6859fb5514ae034e..eef339a9aa0979d8704635abdbe0a815587a0650 100644 --- a/VirtualRobot/MathTools.cpp +++ b/VirtualRobot/MathTools.cpp @@ -1,8 +1,8 @@ #include "MathTools.h" #include "VirtualRobotException.h" -#include <float.h> -#include <string.h> +#include <cfloat> +#include <cstring> #include <cmath> #include <fstream> #include <iomanip> @@ -849,15 +849,10 @@ namespace VirtualRobot void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::convertMM2M(const std::vector<ContactPoint> points, std::vector<ContactPoint>& storeResult) { float s = 1.0f / 1000.0f; - storeResult.clear(); - std::vector<ContactPoint>::const_iterator iter1 = points.begin(); - - while (iter1 != points.end()) + storeResult = points; + for(auto& p : storeResult) { - ContactPoint tmp = *iter1; - tmp.p *= s; - storeResult.push_back(tmp); - iter1++; + p.p *= s; } } @@ -948,9 +943,9 @@ namespace VirtualRobot void VIRTUAL_ROBOT_IMPORT_EXPORT MathTools::print(const std::vector<ContactPoint>& points) { - for (size_t i = 0; i < points.size(); i++) + for (const auto & point : points) { - print(points[i]); + print(point); } } @@ -1494,21 +1489,21 @@ namespace VirtualRobot res.x = res.y = res.z = res.w = 0; - for (size_t i = 0; i < quaternions.size(); i++) + for (auto & quaternion : quaternions) { - if (getDot(res, quaternions[i]) > 0) + if (getDot(res, quaternion) > 0) { - res.x += quaternions[i].x; - res.y += quaternions[i].y; - res.z += quaternions[i].z; - res.w += quaternions[i].w; + res.x += quaternion.x; + res.y += quaternion.y; + res.z += quaternion.z; + res.w += quaternion.w; } else { - res.x += -quaternions[i].x; - res.y += -quaternions[i].y; - res.z += -quaternions[i].z; - res.w += -quaternions[i].w; + res.x += -quaternion.x; + res.y += -quaternion.y; + res.z += -quaternion.z; + res.w += -quaternion.w; } } diff --git a/VirtualRobot/Nodes/CameraSensor.cpp b/VirtualRobot/Nodes/CameraSensor.cpp index 530cc5a4eaf090b48fd634763d29245d765e8bce..5f8a6f1038d20422c303165bb8dc5581b183f386 100644 --- a/VirtualRobot/Nodes/CameraSensor.cpp +++ b/VirtualRobot/Nodes/CameraSensor.cpp @@ -18,8 +18,7 @@ namespace VirtualRobot CameraSensor::~CameraSensor() - { - } + = default; diff --git a/VirtualRobot/Nodes/CameraSensorFactory.cpp b/VirtualRobot/Nodes/CameraSensorFactory.cpp index 265250e0020f6cfc0afaf77bf8029158c1a2acc1..a667d1e2e718876251b80b6d580375faf36cfd55 100644 --- a/VirtualRobot/Nodes/CameraSensorFactory.cpp +++ b/VirtualRobot/Nodes/CameraSensorFactory.cpp @@ -11,13 +11,11 @@ namespace VirtualRobot { CameraSensorFactory::CameraSensorFactory() - { - } + = default; CameraSensorFactory::~CameraSensorFactory() - { - } + = default; /** diff --git a/VirtualRobot/Nodes/ContactSensor.cpp b/VirtualRobot/Nodes/ContactSensor.cpp index 98d3873bcfdc06543ffe5a47ef6a673dd42a46a5..5f78e2ab576bb22c72da12b95e73622d73a9f31e 100644 --- a/VirtualRobot/Nodes/ContactSensor.cpp +++ b/VirtualRobot/Nodes/ContactSensor.cpp @@ -17,8 +17,7 @@ namespace VirtualRobot ContactSensor::~ContactSensor() - { - } + = default; void ContactSensor::print(bool printChildren, bool printDecoration) const { diff --git a/VirtualRobot/Nodes/ContactSensorFactory.cpp b/VirtualRobot/Nodes/ContactSensorFactory.cpp index 2af149f3fe0e52568ed06826920f5fab4326556f..1b1fac58d67ca595f92e16c7fa5150de57ff9b25 100644 --- a/VirtualRobot/Nodes/ContactSensorFactory.cpp +++ b/VirtualRobot/Nodes/ContactSensorFactory.cpp @@ -11,13 +11,11 @@ namespace VirtualRobot { ContactSensorFactory::ContactSensorFactory() - { - } + = default; ContactSensorFactory::~ContactSensorFactory() - { - } + = default; /** diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.cpp b/VirtualRobot/Nodes/ForceTorqueSensor.cpp index 6785283a5990a3a782db1664ea302c6d4debba3c..67d1be48e9e2dbca10986aad8f81674caf790b7d 100644 --- a/VirtualRobot/Nodes/ForceTorqueSensor.cpp +++ b/VirtualRobot/Nodes/ForceTorqueSensor.cpp @@ -19,8 +19,7 @@ namespace VirtualRobot ForceTorqueSensor::~ForceTorqueSensor() - { - } + = default; Eigen::Vector3f ForceTorqueSensor::getForce() const { diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp index d1aba5324ae6c87122dce67daa5dcf0855224a4f..4958aed4b1fff7a27b8173886c9e9cafbfca95fc 100644 --- a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp +++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp @@ -11,13 +11,11 @@ namespace VirtualRobot { ForceTorqueSensorFactory::ForceTorqueSensorFactory() - { - } + = default; ForceTorqueSensorFactory::~ForceTorqueSensorFactory() - { - } + = default; /** diff --git a/VirtualRobot/Nodes/PositionSensor.cpp b/VirtualRobot/Nodes/PositionSensor.cpp index cdf4d044fd6099643968e92cd5a87e2494489a17..136581a49e129a32e0e635c8eac359d189469866 100644 --- a/VirtualRobot/Nodes/PositionSensor.cpp +++ b/VirtualRobot/Nodes/PositionSensor.cpp @@ -19,8 +19,7 @@ namespace VirtualRobot PositionSensor::~PositionSensor() - { - } + = default; diff --git a/VirtualRobot/Nodes/PositionSensorFactory.cpp b/VirtualRobot/Nodes/PositionSensorFactory.cpp index 3f908106cb128efd03bbeb94e71bd51c20274ca0..532bb418bbcbe99641231100ea6bbb28a4c5a065 100644 --- a/VirtualRobot/Nodes/PositionSensorFactory.cpp +++ b/VirtualRobot/Nodes/PositionSensorFactory.cpp @@ -11,13 +11,11 @@ namespace VirtualRobot { PositionSensorFactory::PositionSensorFactory() - { - } + = default; PositionSensorFactory::~PositionSensorFactory() - { - } + = default; /** diff --git a/VirtualRobot/Nodes/RobotNodeActuator.cpp b/VirtualRobot/Nodes/RobotNodeActuator.cpp index 4f10f65ef88546b8cee84a3b8bc0fa2d6601e6d4..8276bf130e88043551cbf95028642c77d72edad8 100644 --- a/VirtualRobot/Nodes/RobotNodeActuator.cpp +++ b/VirtualRobot/Nodes/RobotNodeActuator.cpp @@ -11,8 +11,7 @@ namespace VirtualRobot } RobotNodeActuator::~RobotNodeActuator() - { - } + = default; void RobotNodeActuator::updateVisualizationPose(const Eigen::Matrix4f& pose, bool updateChildren) { diff --git a/VirtualRobot/Nodes/RobotNodeFixed.cpp b/VirtualRobot/Nodes/RobotNodeFixed.cpp index 513726f9b6ab6f90559e33878112ec7aaa9a953f..b7306251f0756553b32b292835acd02684518ea2 100644 --- a/VirtualRobot/Nodes/RobotNodeFixed.cpp +++ b/VirtualRobot/Nodes/RobotNodeFixed.cpp @@ -63,8 +63,7 @@ namespace VirtualRobot } RobotNodeFixed::~RobotNodeFixed() - { - } + = default; bool RobotNodeFixed::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children) { diff --git a/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp b/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp index 9aa667ca56218a83483dc577ecaa7eed7fe930d4..f6cef81580920d66abaffada7410c3434ba78a28 100644 --- a/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp +++ b/VirtualRobot/Nodes/RobotNodeFixedFactory.cpp @@ -13,13 +13,11 @@ namespace VirtualRobot { RobotNodeFixedFactory::RobotNodeFixedFactory() - { - } + = default; RobotNodeFixedFactory::~RobotNodeFixedFactory() - { - } + = default; /** * This method creates a VirtualRobot::RobotNodeFixed. @@ -28,7 +26,7 @@ namespace VirtualRobot */ RobotNodePtr RobotNodeFixedFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float /*limitLow*/, float /*limitHigh*/, float /*jointValueOffset*/, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& /*axis*/, const Eigen::Vector3f& /*translationDirection*/, const SceneObject::Physics& p, RobotNode::RobotNodeType rntype) const { - RobotNodePtr robotNode(new RobotNodeFixed(robot, nodeName, preJointTransform, visualizationModel, collisionModel, p, CollisionCheckerPtr(), rntype)); + RobotNodePtr robotNode(new RobotNodeFixed(robot, nodeName, preJointTransform, visualizationModel, collisionModel, p, (collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr()), rntype)); return robotNode; } diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.cpp b/VirtualRobot/Nodes/RobotNodePrismatic.cpp index 5e26ae82b3755ce992f6970135b9bcdc17f7d731..b5b86fda3aa7cde71bf22f819b910c38707ce751 100644 --- a/VirtualRobot/Nodes/RobotNodePrismatic.cpp +++ b/VirtualRobot/Nodes/RobotNodePrismatic.cpp @@ -77,8 +77,7 @@ namespace VirtualRobot } RobotNodePrismatic::~RobotNodePrismatic() - { - } + = default; bool RobotNodePrismatic::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children) diff --git a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp index c447bde2ca645e3c3d464947a2cdc8a7c28c0e31..4bb87ebdccb2c39e974d142d85716db8096e124a 100644 --- a/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp +++ b/VirtualRobot/Nodes/RobotNodePrismaticFactory.cpp @@ -13,13 +13,11 @@ namespace VirtualRobot { RobotNodePrismaticFactory::RobotNodePrismaticFactory() - { - } + = default; RobotNodePrismaticFactory::~RobotNodePrismaticFactory() - { - } + = default; /** @@ -29,7 +27,7 @@ namespace VirtualRobot */ RobotNodePtr RobotNodePrismaticFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& /*axis*/, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p, RobotNode::RobotNodeType rntype) const { - RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, limitLow, limitHigh, preJointTransform, translationDirection, visualizationModel, collisionModel, jointValueOffset, p, CollisionCheckerPtr(), rntype)); + RobotNodePtr robotNode(new RobotNodePrismatic(robot, nodeName, limitLow, limitHigh, preJointTransform, translationDirection, visualizationModel, collisionModel, jointValueOffset, p, (collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr()), rntype)); return robotNode; } diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.cpp b/VirtualRobot/Nodes/RobotNodeRevolute.cpp index ba8e6c7715b629dcd17b1ab7635b2c559d1f168b..4547b8475ebc0be7a0073e40a609dc3ffa3484cd 100644 --- a/VirtualRobot/Nodes/RobotNodeRevolute.cpp +++ b/VirtualRobot/Nodes/RobotNodeRevolute.cpp @@ -79,8 +79,7 @@ namespace VirtualRobot RobotNodeRevolute::~RobotNodeRevolute() - { - } + = default; bool RobotNodeRevolute::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children) { diff --git a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp index 00d5f9986be6a37ee4b639bf411bd9c60208609b..40e79deda46c94fa0cd3e7bda1f04e511379562c 100644 --- a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp +++ b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.cpp @@ -13,13 +13,11 @@ namespace VirtualRobot { RobotNodeRevoluteFactory::RobotNodeRevoluteFactory() - { - } + = default; RobotNodeRevoluteFactory::~RobotNodeRevoluteFactory() - { - } + = default; /** @@ -29,7 +27,7 @@ namespace VirtualRobot */ RobotNodePtr RobotNodeRevoluteFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& /*translationDirection*/, const SceneObject::Physics& p, RobotNode::RobotNodeType rntype) const { - RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, preJointTransform, axis, visualizationModel, collisionModel, jointValueOffset, p, CollisionCheckerPtr(), rntype)); + RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, preJointTransform, axis, visualizationModel, collisionModel, jointValueOffset, p, (collisionModel ? collisionModel->getCollisionChecker() : CollisionCheckerPtr()), rntype)); return robotNode; } diff --git a/VirtualRobot/Nodes/Sensor.cpp b/VirtualRobot/Nodes/Sensor.cpp index f0af61ac26b4a6b01d9b232af9a6c7756180a39f..5dd29e474b7cabb222666602e811faa54d600a58 100644 --- a/VirtualRobot/Nodes/Sensor.cpp +++ b/VirtualRobot/Nodes/Sensor.cpp @@ -25,8 +25,7 @@ namespace VirtualRobot Sensor::~Sensor() - { - } + = default; bool Sensor::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children) @@ -112,9 +111,9 @@ namespace VirtualRobot { std::vector< SceneObjectPtr > children = this->getChildren(); - for (unsigned int i = 0; i < children.size(); i++) + for (auto & i : children) { - children[i]->print(true, true); + i->print(true, true); } } } diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp index 6df92ebece934a395fbe63e502f4bce1dd7a06cb..94dbaea9fbf0347313c217de0c5ebf83559ea891 100644 --- a/VirtualRobot/Obstacle.cpp +++ b/VirtualRobot/Obstacle.cpp @@ -41,8 +41,7 @@ namespace VirtualRobot } Obstacle::~Obstacle() - { - } + = default; int Obstacle::getID() { diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp index 857e9fc903ed06f7cf184c79aa14b1ee0c976c2d..6775ce8da804b26a450098a908b05d4a8eca8d6c 100644 --- a/VirtualRobot/Robot.cpp +++ b/VirtualRobot/Robot.cpp @@ -20,12 +20,10 @@ namespace VirtualRobot } Robot::Robot() - { - } + = default; Robot::~Robot() - { - } + = default; LocalRobot::~LocalRobot() { @@ -36,9 +34,9 @@ namespace VirtualRobot { std::vector<SceneObjectPtr> c = it->second->getChildren(); - for (size_t j = 0; j < c.size(); j++) + for (const auto & j : c) { - it->second->detachChild(c[j]); + it->second->detachChild(j); } it++; @@ -75,14 +73,14 @@ namespace VirtualRobot std::vector< RobotNodePtr > allNodes; node->collectAllRobotNodes(allNodes); - for (unsigned int i = 0; i < allNodes.size(); i++) + for (auto & allNode : allNodes) { - std::string name = allNodes[i]->getName(); + std::string name = allNode->getName(); if (!this->hasRobotNode(name)) { VR_WARNING << "Robot node with name <" << name << "> was not registered, adding it to RobotNodeMap" << endl; - registerRobotNode(allNodes[i]); + registerRobotNode(allNode); } } } @@ -611,7 +609,7 @@ namespace VirtualRobot while (robotNodes.end() != iterator) { - (*iterator)->showCoordinateSystem(enable, 1.0f, NULL, type); + (*iterator)->showCoordinateSystem(enable, 1.0f, nullptr, type); ++iterator; } } @@ -813,13 +811,13 @@ namespace VirtualRobot std::vector<RobotNodePtr> allNodes; startJoint->collectAllRobotNodes(allNodes); - for (size_t i = 0; i < allNodes.size(); i++) + for (auto & allNode : allNodes) { - RobotNodePtr roN = result->getRobotNode(allNodes[i]->getName()); + RobotNodePtr roN = result->getRobotNode(allNode->getName()); if (roN) { - roN->setJointValueNoUpdate(allNodes[i]->getJointValue()); + roN->setJointValueNoUpdate(allNode->getJointValue()); } } @@ -1043,15 +1041,15 @@ namespace VirtualRobot VirtualRobot::BoundingBox bbox; std::vector<RobotNodePtr> rn = getRobotNodes(); - for (size_t i = 0; i < rn.size(); i++) + for (auto & i : rn) { - if (collisionModel && rn[i]->getCollisionModel()) + if (collisionModel && i->getCollisionModel()) { - bbox.addPoints(rn[i]->getCollisionModel()->getBoundingBox()); + bbox.addPoints(i->getCollisionModel()->getBoundingBox()); } - else if (!collisionModel && rn[i]->getVisualization()) + else if (!collisionModel && i->getVisualization()) { - bbox.addPoints(rn[i]->getVisualization()->getBoundingBox()); + bbox.addPoints(i->getVisualization()->getBoundingBox()); } } @@ -1062,15 +1060,15 @@ namespace VirtualRobot { std::vector<RobotNodePtr> rn = getRobotNodes(); - for (size_t i = 0; i < rn.size(); i++) + for (auto & i : rn) { - std::vector<SensorPtr> sensors = rn[i]->getSensors(); + std::vector<SensorPtr> sensors = i->getSensors(); - for (std::vector<SensorPtr>::iterator it = sensors.begin(); it != sensors.end(); it++) + for (auto & sensor : sensors) { - if ((*it)->getName() == name) + if (sensor->getName() == name) { - return *it; + return sensor; } } } @@ -1083,9 +1081,9 @@ namespace VirtualRobot std::vector<SensorPtr> result; std::vector<RobotNodePtr> rn = getRobotNodes(); - for (size_t i = 0; i < rn.size(); i++) + for (auto & i : rn) { - std::vector<SensorPtr> s = rn[i]->getSensors(); + std::vector<SensorPtr> s = i->getSensors(); if (s.size() > 0) { @@ -1104,9 +1102,9 @@ namespace VirtualRobot ss << "<Robot Type='" << this->type << "' RootNode='" << this->getRootNode()->getName() << "'>" << endl << endl; std::vector<RobotNodePtr> nodes = getRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - ss << nodes[i]->toXML(basePath, modelPath, storeSensors) << endl; + ss << node->toXML(basePath, modelPath, storeSensors) << endl; } ss << endl; @@ -1116,9 +1114,9 @@ namespace VirtualRobot std::vector<RobotNodeSetPtr> rns; this->getRobotNodeSets(rns); - for (size_t i = 0; i < rns.size(); i++) + for (auto & rn : rns) { - ss << rns[i]->toXML(1) << endl; + ss << rn->toXML(1) << endl; } if (rns.size() > 0) @@ -1131,9 +1129,9 @@ namespace VirtualRobot { std::vector<EndEffectorPtr> eefs = this->getEndEffectors(); - for (size_t i = 0; i < eefs.size(); i++) + for (auto & eef : eefs) { - ss << eefs[i]->toXML(1) << endl; + ss << eef->toXML(1) << endl; } if (eefs.size() > 0) diff --git a/VirtualRobot/RobotConfig.cpp b/VirtualRobot/RobotConfig.cpp index 2e32d380219d531c692bb489f1684dabb477ee7c..dd0ce49f7c47af379d4ff5b72c1657a523e2ed92 100644 --- a/VirtualRobot/RobotConfig.cpp +++ b/VirtualRobot/RobotConfig.cpp @@ -20,9 +20,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!robot.lock(), "NULL robot in RobotConfig"); - for (std::vector< Configuration >::const_iterator i = configs.begin(); i != configs.end(); i++) + for (const auto & config : configs) { - setConfig((*i)); + setConfig(config); } } @@ -32,9 +32,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!robot.lock(), "NULL robot in RobotConfig"); - for (std::map< RobotNodePtr, float >::const_iterator i = configs.begin(); i != configs.end(); i++) + for (const auto & config : configs) { - setConfig(i->first, i->second); + setConfig(config.first, config.second); } } @@ -69,9 +69,9 @@ namespace VirtualRobot { cout << " Robot Config <" << name << ">" << endl; - for (std::map< RobotNodePtr, float >::const_iterator i = configs.begin(); i != configs.end(); i++) + for (const auto & config : configs) { - cout << " * " << i->first->getName() << ":\t" << i->second << endl; + cout << " * " << config.first->getName() << ":\t" << config.second << endl; } } @@ -188,9 +188,9 @@ namespace VirtualRobot bool RobotConfig::hasConfig(const std::string& name) const { - for (std::map< RobotNodePtr, float >::const_iterator i = configs.begin(); i != configs.end(); i++) + for (const auto & config : configs) { - if (i->first->getName() == name) + if (config.first->getName() == name) { return true; } diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp index 6e2bf1e75a59008f7d1fc883310ad8935bcc9fd9..00c2270b613d9d42fa89e3e27a278915d8d33894 100644 --- a/VirtualRobot/RobotFactory.cpp +++ b/VirtualRobot/RobotFactory.cpp @@ -20,12 +20,10 @@ namespace VirtualRobot RobotFactory::RobotFactory() - { - } + = default; RobotFactory::~RobotFactory() - { - } + = default; RobotPtr RobotFactory::createRobot(const std::string& name, const std::string& type) @@ -73,10 +71,8 @@ namespace VirtualRobot std::vector< std::string > childNames = iterC->second; RobotNodePtr node = iterC->first; - for (size_t i = 0; i < childNames.size(); i++) + for (auto childName : childNames) { - std::string childName = childNames[i]; - if (!robot->hasRobotNode(childName)) { THROW_VR_EXCEPTION("Robot " << robot->getName() << ": corrupted RobotNode <" << node->getName() << " child :" << childName << " does not exist..."); @@ -92,11 +88,11 @@ namespace VirtualRobot // register root (performs an initialization of all robot nodes) robot->setRootNode(rootNode); - for (size_t i = 0; i < robotNodes.size(); i++) + for (auto & robotNode : robotNodes) { - if (!robotNodes[i]->getParent() && robotNodes[i] != rootNode) + if (!robotNode->getParent() && robotNode != rootNode) { - VR_ERROR << "RobotNode " << robotNodes[i]->getName() << " is not connected to kinematic structure..." << endl; + VR_ERROR << "RobotNode " << robotNode->getName() << " is not connected to kinematic structure..." << endl; } } @@ -163,11 +159,11 @@ namespace VirtualRobot edges.push_back(edge); } - for (unsigned i = 0; i < children.size(); i++) + for (auto & i : children) { - if (children[i] != currentEdge.first) + if (i != currentEdge.first) { - RobotNodePtr childNode = boost::dynamic_pointer_cast<RobotNode>(children[i]); + RobotNodePtr childNode = boost::dynamic_pointer_cast<RobotNode>(i); // not a robot node if (!childNode) @@ -175,7 +171,7 @@ namespace VirtualRobot continue; } - rnDef.children.push_back(children[i]->getName()); + rnDef.children.push_back(i->getName()); rnDef.invertTransformation.push_back(false); RobotTreeEdge edge; edge.second = childNode; @@ -304,10 +300,10 @@ namespace VirtualRobot if (o->getCollisionModel()) { - c = o->getCollisionModel()->clone(); + c = o->getCollisionModel(); } - auto rnf = RobotNodeFixedFactory::createInstance(NULL); + auto rnf = RobotNodeFixedFactory::createInstance(nullptr); RobotNodePtr newRN = rnf->createRobotNode(robot, name, v, c, 0, 0, 0, transformation, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), p); rn->attachChild(newRN); newRN->initialize(rn); @@ -362,15 +358,15 @@ namespace VirtualRobot std::map<RobotNodePtr, std::vector<SensorPtr> > sensorMap; std::map<RobotNodePtr, bool> directionInversion; - for (size_t i = 0; i < newStructure.parentChildMapping.size(); i++) + for (auto & i : newStructure.parentChildMapping) { - if (!robot->hasRobotNode(newStructure.parentChildMapping[i].name)) + if (!robot->hasRobotNode(i.name)) { - VR_ERROR << "Error in parentChildMapping, no node with name " << newStructure.parentChildMapping[i].name << endl; + VR_ERROR << "Error in parentChildMapping, no node with name " << i.name << endl; return RobotPtr(); } - nodeName = newStructure.parentChildMapping[i].name; + nodeName = i.name; if (newNodes.find(nodeName) == newNodes.end()) { @@ -379,11 +375,11 @@ namespace VirtualRobot newNodes[nodeName] = rn; } - RobotNodePtr parent = newNodes[newStructure.parentChildMapping[i].name]; + RobotNodePtr parent = newNodes[i.name]; - for (size_t j = 0; j < newStructure.parentChildMapping[i].children.size(); j++) + for (size_t j = 0; j < i.children.size(); j++) { - nodeName = newStructure.parentChildMapping[i].children[j]; + nodeName = i.children[j]; if (!robot->hasRobotNode(nodeName)) { @@ -402,7 +398,7 @@ namespace VirtualRobot RobotNodePtr child = newNodes[nodeName]; parent->attachChild(child); - if (newStructure.parentChildMapping[i].invertTransformation[j]) + if (i.invertTransformation[j]) { Eigen::Matrix4f tr = parent->getLocalTransformation().inverse(); localTransformations[child] = tr; @@ -419,9 +415,9 @@ namespace VirtualRobot vf->applyDisplacement(v, tr2); visuMap[parent] = v; - for (size_t pr = 0; pr < v->primitives.size(); pr++) + for (auto & primitive : v->primitives) { - v->primitives[pr]->transform = tr * v->primitives[pr]->transform; + primitive->transform = tr * primitive->transform; } } @@ -437,9 +433,9 @@ namespace VirtualRobot c->setVisualization(v); colMap[parent] = c; - for (size_t pr = 0; pr < v->primitives.size(); pr++) + for (auto & primitive : v->primitives) { - v->primitives[pr]->transform = tr * v->primitives[pr]->transform; + primitive->transform = tr * primitive->transform; } } @@ -545,38 +541,38 @@ namespace VirtualRobot std::vector<RobotNodePtr> nodes = newRobot->getRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - if (visuMap.find(nodes[i]) != visuMap.end()) + if (visuMap.find(node) != visuMap.end()) { - nodes[i]->setVisualization(visuMap[nodes[i]]); + node->setVisualization(visuMap[node]); } else { - nodes[i]->setVisualization(VisualizationNodePtr()); + node->setVisualization(VisualizationNodePtr()); } - if (colMap.find(nodes[i]) != colMap.end()) + if (colMap.find(node) != colMap.end()) { - nodes[i]->setCollisionModel(colMap[nodes[i]]); + node->setCollisionModel(colMap[node]); } else { - nodes[i]->setCollisionModel(CollisionModelPtr()); + node->setCollisionModel(CollisionModelPtr()); } - if (physicsMap.find(nodes[i]) != physicsMap.end()) + if (physicsMap.find(node) != physicsMap.end()) { - nodes[i]->physics = physicsMap[nodes[i]]; + node->physics = physicsMap[node]; } - if (sensorMap.find(nodes[i]) != sensorMap.end()) + if (sensorMap.find(node) != sensorMap.end()) { - auto sensors = sensorMap[nodes[i]]; + auto sensors = sensorMap[node]; for (auto s : sensors) { - nodes[i]->registerSensor(s); + node->registerSensor(s); } } } @@ -607,9 +603,8 @@ namespace VirtualRobot std::vector < SceneObjectPtr > children = nodeA->getChildren(); std::vector<RobotNodePtr> childNodes; - for (size_t i = 0; i < children.size(); i++) + for (auto c : children) { - SceneObjectPtr c = children[i]; RobotNodePtr cRN = boost::dynamic_pointer_cast<RobotNode>(c); if (cRN && cRN != nodeExclude) @@ -626,9 +621,8 @@ namespace VirtualRobot std::vector < SceneObjectPtr > children = nodeA->getChildren(); std::vector<RobotNodePtr> childNodes; - for (size_t i = 0; i < children.size(); i++) + for (auto c : children) { - SceneObjectPtr c = children[i]; SensorPtr cS = boost::dynamic_pointer_cast<Sensor>(c); RobotNodePtr cRN = boost::dynamic_pointer_cast<RobotNode>(c); @@ -648,7 +642,7 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!robot, "NULL data"); THROW_VR_EXCEPTION_IF(!nodeA, "NULL data"); - auto rnf = RobotNodeFixedFactory::createInstance(NULL); + auto rnf = RobotNodeFixedFactory::createInstance(nullptr); SceneObject::Physics p; VisualizationNodePtr v; CollisionModelPtr c; @@ -688,7 +682,7 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!robot, "NULL data"); - auto rnf = RobotNodeFixedFactory::createInstance(NULL); + auto rnf = RobotNodeFixedFactory::createInstance(nullptr); SceneObject::Physics p; VisualizationNodePtr v; CollisionModelPtr c; @@ -709,9 +703,9 @@ namespace VirtualRobot } // attach sensors - for (size_t i = 0; i < sensors.size(); i++) + for (const auto & sensor : sensors) { - SensorPtr s = sensors[i]->clone(newRN); + SensorPtr s = sensor->clone(newRN); } return newRN; @@ -723,19 +717,19 @@ namespace VirtualRobot float kg = 0; - for (size_t i = 0; i < nodes.size(); i++) + for (const auto & node : nodes) { - if (nodes[i]->getVisualization()) + if (node->getVisualization()) { - visus.push_back(nodes[i]->getVisualization()); + visus.push_back(node->getVisualization()); } - if (nodes[i]->getCollisionModel() && nodes[i]->getCollisionModel()->getVisualization()) + if (node->getCollisionModel() && node->getCollisionModel()->getVisualization()) { - colVisus.push_back(nodes[i]->getCollisionModel()->getVisualization()); + colVisus.push_back(node->getCollisionModel()->getVisualization()); } - kg += nodes[i]->getMass(); + kg += node->getMass(); } if (visus.size() > 0) @@ -766,11 +760,11 @@ namespace VirtualRobot newRN->initialize(parentClone); // attach sensors - for (size_t i = 0; i < sensors.size(); i++) + for (const auto & sensor : sensors) { - SensorPtr s = sensors[i]->clone(newRN); + SensorPtr s = sensor->clone(newRN); Eigen::Matrix4f trafoToNewRN = parent?parent->getGlobalPose() * trafo:trafo; - Eigen::Matrix4f t = trafoToNewRN.inverse() * sensors[i]->getGlobalPose(); + Eigen::Matrix4f t = trafoToNewRN.inverse() * sensor->getGlobalPose(); s->setRobotNodeToSensorTransformation(t); } @@ -816,13 +810,12 @@ namespace VirtualRobot std::vector< RobotNodePtr > initialNodes = startNode->getAllParents(); // check for static nodes which are not parent of startNode std::vector< RobotNodePtr > allNodes = robot->getRobotNodes(); - for (size_t i=0;i<allNodes.size();i++) + for (auto rn : allNodes) { - RobotNodePtr rn = allNodes[i]; bool isFixed = true; - for (size_t j = 0; j < nodes.size(); j++) + for (const auto & node : nodes) { - if (rn->hasChild(nodes[j],true)) + if (rn->hasChild(node,true)) { isFixed = false; break; @@ -847,13 +840,12 @@ namespace VirtualRobot // collect sensor nodes std::vector<SensorPtr> childSensorNodes; - for (size_t i = 0; i < initialNodes.size(); i++) + for (auto rn : initialNodes) { - RobotNodePtr rn = initialNodes[i]; std::vector<SceneObjectPtr> c = rn->getChildren(); - for (size_t j = 0; j < c.size(); j++) + for (const auto & j : c) { - SensorPtr s = boost::dynamic_pointer_cast<Sensor>(c[j]); + SensorPtr s = boost::dynamic_pointer_cast<Sensor>(j); if (s) childSensorNodes.push_back(s); } @@ -899,9 +891,9 @@ namespace VirtualRobot if (uniteWithAllChildren.size() == 0) return RobotFactory::clone(robot, robot->getName()); - for (size_t i = 0; i < uniteWithAllChildren.size(); i++) + for (const auto & i : uniteWithAllChildren) { - THROW_VR_EXCEPTION_IF(!robot->hasRobotNode(uniteWithAllChildren[i]), "Could not find RobotNode in robot"); + THROW_VR_EXCEPTION_IF(!robot->hasRobotNode(i), "Could not find RobotNode in robot"); } @@ -916,21 +908,19 @@ namespace VirtualRobot // clone RNS std::vector<RobotNodeSetPtr> rnsets = robot->getRobotNodeSets(); - for (size_t i = 0; i < rnsets.size(); i++) + for (auto rns : rnsets) { - RobotNodeSetPtr rns = rnsets[i]; - bool ok = true; - for (size_t j = 0; j < uniteWithAllChildren.size(); j++) + for (const auto & j : uniteWithAllChildren) { - RobotNodePtr rn = robot->getRobotNode(uniteWithAllChildren[j]); + RobotNodePtr rn = robot->getRobotNode(j); std::vector<RobotNodePtr> allChildren; rn->collectAllRobotNodes(allChildren); - for (size_t k = 0; k < allChildren.size(); k++) + for (auto & k : allChildren) { - if (allChildren[k] == rn) + if (k == rn) continue; - if (rns->hasRobotNode(allChildren[k]->getName())) + if (rns->hasRobotNode(k->getName())) { ok = false; break; @@ -952,11 +942,11 @@ namespace VirtualRobot std::vector<SceneObjectPtr> c = currentNode->getChildren(); - for (size_t i = 0; i < c.size(); i++) + for (auto & i : c) { - if (std::find(uniteWithAllChildren.begin(), uniteWithAllChildren.end(), c[i]->getName()) != uniteWithAllChildren.end()) + if (std::find(uniteWithAllChildren.begin(), uniteWithAllChildren.end(), i->getName()) != uniteWithAllChildren.end()) { - RobotNodePtr currentRN = boost::dynamic_pointer_cast<RobotNode>(c[i]); + RobotNodePtr currentRN = boost::dynamic_pointer_cast<RobotNode>(i); THROW_VR_EXCEPTION_IF(!currentRN, "Only RN allowed in list"); RobotNodePtr currentRNClone = currentRN->clone(robot, false, currentNodeClone); @@ -974,7 +964,7 @@ namespace VirtualRobot } else { - RobotNodePtr currentRN = boost::dynamic_pointer_cast<RobotNode>(c[i]); + RobotNodePtr currentRN = boost::dynamic_pointer_cast<RobotNode>(i); if (currentRN) { RobotNodePtr currentRNClone = currentRN->clone(robot, false, currentNodeClone); @@ -982,12 +972,12 @@ namespace VirtualRobot } else { - SensorPtr s = boost::dynamic_pointer_cast<Sensor>(c[i]); + SensorPtr s = boost::dynamic_pointer_cast<Sensor>(i); if (s) { s->clone(currentNodeClone); } else - VR_INFO << "Skipping node " << c[i]->getName() << endl; + VR_INFO << "Skipping node " << i->getName() << endl; } } diff --git a/VirtualRobot/RobotNodeSet.cpp b/VirtualRobot/RobotNodeSet.cpp index 480a8fac63614838422b18b84d138f85480c9067..38cc155eca8bdaff7133ca297b4826dd770a2f8a 100644 --- a/VirtualRobot/RobotNodeSet.cpp +++ b/VirtualRobot/RobotNodeSet.cpp @@ -60,15 +60,15 @@ namespace VirtualRobot // now, the objects are stored in the parent's (SceneObjectSet) data structure, so that the methods of SceneObjectSet do work - for (size_t i = 0; i < robotNodes.size(); i++) + for (const auto & robotNode : robotNodes) { - SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNodes[i]); + SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNode); if (cm) { if (colChecker != cm->getCollisionChecker()) { - VR_WARNING << "col model of " << robotNodes[i]->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl; + VR_WARNING << "col model of " << robotNode->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl; } else { @@ -86,7 +86,7 @@ namespace VirtualRobot const std::string& tcpName, bool registerToRobot) { - VR_ASSERT(robot != NULL); + VR_ASSERT(robot != nullptr); std::vector< RobotNodePtr > robotNodes; if (robotNodeNames.empty()) @@ -95,10 +95,10 @@ namespace VirtualRobot } else { - for (unsigned int i = 0; i < robotNodeNames.size(); i++) + for (const auto & robotNodeName : robotNodeNames) { - RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]); - THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found..."); + RobotNodePtr node = robot->getRobotNode(robotNodeName); + THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeName << " found..."); robotNodes.push_back(node); } } @@ -147,7 +147,7 @@ namespace VirtualRobot const RobotNodePtr tcp, bool registerToRobot) { - VR_ASSERT(robot != NULL); + VR_ASSERT(robot != nullptr); if (robotNodes.empty() || !robotNodes[0]) { @@ -249,9 +249,9 @@ namespace VirtualRobot bool RobotNodeSet::hasRobotNode(RobotNodePtr robotNode) const { - for (unsigned int i = 0; i < robotNodes.size(); i++) + for (const auto & i : robotNodes) { - if (robotNodes[i] == robotNode) + if (i == robotNode) { return true; } @@ -261,9 +261,9 @@ namespace VirtualRobot bool RobotNodeSet::hasRobotNode(const std::string &nodeName) const { - for (unsigned int i = 0; i < robotNodes.size(); i++) + for (const auto & robotNode : robotNodes) { - if (robotNodes[i]->getName() == nodeName) + if (robotNode->getName() == nodeName) { return true; } @@ -361,9 +361,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!fillVector, "NULL data"); - for (size_t i = 0; i < robotNodes.size(); i++) + for (const auto & robotNode : robotNodes) { - fillVector->setConfig(robotNodes[i]->getName(), robotNodes[i]->getJointValue()); + fillVector->setConfig(robotNode->getName(), robotNode->getJointValue()); } } @@ -463,11 +463,11 @@ namespace VirtualRobot THROW_VR_EXCEPTION_IF(!rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)"); WriteLockPtr lock = rob->getWriteLock(); - for (unsigned int i = 0; i < robotNodes.size(); i++) + for (auto & robotNode : robotNodes) { - if (jointValues->hasConfig(robotNodes[i]->getName())) + if (jointValues->hasConfig(robotNode->getName())) { - robotNodes[i]->setJointValueNoUpdate(jointValues->getConfig(robotNodes[i]->getName())); + robotNode->setJointValueNoUpdate(jointValues->getConfig(robotNode->getName())); } } @@ -744,9 +744,9 @@ namespace VirtualRobot { RobotNodePtr node; VR_ASSERT(robotNode); - for (size_t i = 0; i < robotNodes.size(); i++) + for (const auto & i : robotNodes) { - node = robotNodes.at(i); + node = i; if (node != robotNode && !robotNode->hasChild(node, true)) { @@ -772,9 +772,9 @@ namespace VirtualRobot ss << pre << "<RobotNodeSet name='" << name << "'>\n"; - for (size_t i = 0; i < robotNodes.size(); i++) + for (auto & robotNode : robotNodes) { - ss << pre << t << "<Node name='" << robotNodes[i]->getName() << "'/>\n"; + ss << pre << t << "<Node name='" << robotNode->getName() << "'/>\n"; } ss << pre << "</RobotNodeSet>\n"; diff --git a/VirtualRobot/RuntimeEnvironment.cpp b/VirtualRobot/RuntimeEnvironment.cpp index 09048cdd763274834f607ac06f42665a3b3751bf..7ffdb66916c9860ec543b62349234c652433faa1 100644 --- a/VirtualRobot/RuntimeEnvironment.cpp +++ b/VirtualRobot/RuntimeEnvironment.cpp @@ -125,9 +125,9 @@ namespace VirtualRobot // silently skip this error (e.g. device not ready, permission denied etc) } - for (size_t i = 0; i < dataPaths.size(); i++) + for (auto & dataPath : dataPaths) { - boost::filesystem::path p(dataPaths[i]); + boost::filesystem::path p(dataPath); boost::filesystem::path fnComplete = boost::filesystem::operator/(p, fn); @@ -168,9 +168,9 @@ namespace VirtualRobot ("data-path", boost::program_options::value< std::vector< std::string > >()->composing(), "Set data path. Multiple data paths are allowed.") ; - for (size_t i = 0; i < processKeys.size(); i++) + for (auto & processKey : processKeys) desc.add_options() - (processKeys[i].c_str(), boost::program_options::value< std::vector< std::string > >(), processKeys[i].c_str()) + (processKey.c_str(), boost::program_options::value< std::vector< std::string > >(), processKey.c_str()) ; boost::program_options::parsed_options parsed = @@ -187,28 +187,28 @@ namespace VirtualRobot //VR_INFO << "Data paths are: " << endl; std::vector< std::string > dp = vm["data-path"].as< std::vector< std::string > >(); - for (size_t i = 0; i < dp.size(); i++) + for (const auto & i : dp) { - addDataPath(dp[i]); + addDataPath(i); //VR_INFO << dp[i] << "\n"; } } // process generic keys - for (size_t i = 0; i < processKeys.size(); i++) + for (auto & processKey : processKeys) { - if (vm.count(processKeys[i].c_str())) + if (vm.count(processKey.c_str())) { - std::vector< std::string > dp = vm[processKeys[i].c_str()].as< std::vector< std::string > >(); + std::vector< std::string > dp = vm[processKey.c_str()].as< std::vector< std::string > >(); if (dp.size() > 1) { - VR_WARNING << "More than one parameter for key " << processKeys[i] << ". taking the first one..." << endl; + VR_WARNING << "More than one parameter for key " << processKey << ". taking the first one..." << endl; } if (dp.size() > 0) { - addKeyValuePair(processKeys[i], dp[0]); // take the first one... + addKeyValuePair(processKey, dp[0]); // take the first one... } } @@ -217,9 +217,9 @@ namespace VirtualRobot // collect unrecognized arguments std::vector<std::string> options = boost::program_options::collect_unrecognized(parsed.options, boost::program_options::include_positional); - for (size_t i = 0; i < options.size(); i++) + for (const auto & option : options) { - unrecognizedOptions.push_back(options[i]); + unrecognizedOptions.push_back(option); } @@ -295,18 +295,18 @@ namespace VirtualRobot cout << " *********** Simox RuntimeEnvironment ************* " << endl; cout << "Data paths:" << endl; - for (size_t i = 0; i < dataPaths.size(); i++) + for (const auto & dataPath : dataPaths) { - cout << " * " << dataPaths[i] << endl; + cout << " * " << dataPath << endl; } if (processKeys.size() > 0) { cout << "Known keys:" << endl; - for (size_t i = 0; i < processKeys.size(); i++) + for (const auto & processKey : processKeys) { - cout << " * " << processKeys[i] << endl; + cout << " * " << processKey << endl; } } @@ -326,9 +326,9 @@ namespace VirtualRobot { cout << "Unrecognized options:" << endl; - for (size_t i = 0; i < unrecognizedOptions.size(); i++) + for (const auto & unrecognizedOption : unrecognizedOptions) { - cout << " * <" << unrecognizedOptions[i] << ">" << endl; + cout << " * <" << unrecognizedOption << ">" << endl; } } diff --git a/VirtualRobot/Scene.cpp b/VirtualRobot/Scene.cpp index 526c13dfa8f0283ac37e46a1659bff12be503b8e..eaa2bb3989fcec274b316e81c8bc02ae4d500500 100644 --- a/VirtualRobot/Scene.cpp +++ b/VirtualRobot/Scene.cpp @@ -224,9 +224,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!robot, "NULL data"); - for (std::vector< RobotPtr >::const_iterator i = robots.begin(); i != robots.end(); i++) + for (const auto & i : robots) { - if (*i == robot) + if (i == robot) { return true; } @@ -237,9 +237,9 @@ namespace VirtualRobot bool Scene::hasRobot(const std::string& name) const { - for (std::vector< RobotPtr >::const_iterator i = robots.begin(); i != robots.end(); i++) + for (const auto & robot : robots) { - if ((*i)->getName() == name) + if (robot->getName() == name) { return true; } @@ -252,9 +252,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!obstacle, "NULL data"); - for (std::vector< ObstaclePtr >::const_iterator i = obstacles.begin(); i != obstacles.end(); i++) + for (const auto & i : obstacles) { - if (*i == obstacle) + if (i == obstacle) { return true; } @@ -265,9 +265,9 @@ namespace VirtualRobot bool Scene::hasObstacle(const std::string& name) const { - for (std::vector< ObstaclePtr >::const_iterator i = obstacles.begin(); i != obstacles.end(); i++) + for (const auto & obstacle : obstacles) { - if ((*i)->getName() == name) + if (obstacle->getName() == name) { return true; } @@ -282,9 +282,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!t, "NULL data"); - for (std::vector< TrajectoryPtr >::const_iterator i = trajectories.begin(); i != trajectories.end(); i++) + for (const auto & trajectorie : trajectories) { - if (*i == t) + if (trajectorie == t) { return true; } @@ -295,9 +295,9 @@ namespace VirtualRobot bool Scene::hasTrajectory(const std::string& name) const { - for (std::vector< TrajectoryPtr >::const_iterator i = trajectories.begin(); i != trajectories.end(); i++) + for (const auto & trajectorie : trajectories) { - if ((*i)->getName() == name) + if (trajectorie->getName() == name) { return true; } @@ -311,9 +311,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!manipulationObject, "NULL data"); - for (std::vector< ManipulationObjectPtr >::const_iterator i = manipulationObjects.begin(); i != manipulationObjects.end(); i++) + for (const auto & i : manipulationObjects) { - if (*i == manipulationObject) + if (i == manipulationObject) { return true; } @@ -324,9 +324,9 @@ namespace VirtualRobot bool Scene::hasManipulationObject(const std::string& name) const { - for (std::vector< ManipulationObjectPtr >::const_iterator i = manipulationObjects.begin(); i != manipulationObjects.end(); i++) + for (const auto & manipulationObject : manipulationObjects) { - if ((*i)->getName() == name) + if (manipulationObject->getName() == name) { return true; } @@ -406,9 +406,9 @@ namespace VirtualRobot void Scene::registerRobotConfig(RobotPtr robot, std::vector<RobotConfigPtr> configs) { - for (size_t i = 0; i < configs.size(); i++) + for (const auto & config : configs) { - registerRobotConfig(robot, configs[i]); + registerRobotConfig(robot, config); } } @@ -463,9 +463,9 @@ namespace VirtualRobot THROW_VR_EXCEPTION_IF(!config, "NULL data"); std::vector< RobotConfigPtr > configs = robotConfigs[robot]; - for (std::vector< RobotConfigPtr >::iterator i = configs.begin(); i != configs.end(); i++) + for (auto & i : configs) { - if (*i == config) + if (i == config) { return true; } @@ -479,9 +479,9 @@ namespace VirtualRobot THROW_VR_EXCEPTION_IF(!robot, "NULL data"); std::vector< RobotConfigPtr > configs = robotConfigs[robot]; - for (std::vector< RobotConfigPtr >::iterator i = configs.begin(); i != configs.end(); i++) + for (auto & config : configs) { - if ((*i)->getName() == name) + if (config->getName() == name) { return true; } @@ -493,11 +493,11 @@ namespace VirtualRobot VirtualRobot::RobotConfigPtr Scene::getRobotConfig(const std::string& robotName, const std::string& name) { - for (std::map< RobotPtr, std::vector< RobotConfigPtr > >::iterator i = robotConfigs.begin(); i != robotConfigs.end(); i++) + for (auto & robotConfig : robotConfigs) { - if (i->first->getName() == robotName) + if (robotConfig.first->getName() == robotName) { - return getRobotConfig(i->first, name); + return getRobotConfig(robotConfig.first, name); } } @@ -510,11 +510,11 @@ namespace VirtualRobot THROW_VR_EXCEPTION_IF(!robot, "NULL data"); std::vector< RobotConfigPtr > configs = robotConfigs[robot]; - for (std::vector< RobotConfigPtr >::iterator i = configs.begin(); i != configs.end(); i++) + for (auto & config : configs) { - if ((*i)->getName() == name) + if (config->getName() == name) { - return *i; + return config; } } @@ -546,11 +546,11 @@ namespace VirtualRobot { std::vector< TrajectoryPtr > res; - for (size_t i = 0; i < trajectories.size(); i++) + for (auto & trajectorie : trajectories) { - if (trajectories[i]->getRobotName() == robotName) + if (trajectorie->getRobotName() == robotName) { - res.push_back(trajectories[i]); + res.push_back(trajectorie); } } @@ -642,9 +642,9 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(!sos, "NULL data"); - for (std::vector< SceneObjectSetPtr >::const_iterator i = sceneObjectSets.begin(); i != sceneObjectSets.end(); i++) + for (const auto & sceneObjectSet : sceneObjectSets) { - if (*i == sos) + if (sceneObjectSet == sos) { return true; } @@ -655,9 +655,9 @@ namespace VirtualRobot bool Scene::hasSceneObjectSet(const std::string& name) const { - for (std::vector< SceneObjectSetPtr >::const_iterator i = sceneObjectSets.begin(); i != sceneObjectSets.end(); i++) + for (const auto & sceneObjectSet : sceneObjectSets) { - if ((*i)->getName() == name) + if (sceneObjectSet->getName() == name) { return true; } @@ -698,12 +698,12 @@ namespace VirtualRobot ss << "\n"; // process robots - for (size_t i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - std::string rob = robots[i]->getName(); - RobotConfigPtr currentConfig = robots[i]->getConfig(); + std::string rob = robot->getName(); + RobotConfigPtr currentConfig = robot->getConfig(); ss << "\t<Robot name='" << rob << "' initConfig='" << currentConfig->getName() << "'>\n"; - std::string robFile = robots[i]->getFilename(); + std::string robFile = robot->getFilename(); if (!basePath.empty()) { @@ -713,7 +713,7 @@ namespace VirtualRobot ss << "\t\t<File>" << robFile << "</File>\n"; // store global pose (if not identity) - Eigen::Matrix4f gp = robots[i]->getGlobalPose(); + Eigen::Matrix4f gp = robot->getGlobalPose(); if (!gp.isIdentity()) { @@ -728,11 +728,11 @@ namespace VirtualRobot ss << currentConfig->toXML(2); // store all other configs for robot - std::vector<RobotConfigPtr> rc = getRobotConfigs(robots[i]); + std::vector<RobotConfigPtr> rc = getRobotConfigs(robot); - for (size_t j = 0; j < rc.size(); j++) + for (auto & j : rc) { - ss << rc[j]->toXML(2); + ss << j->toXML(2); } ss << "\t</Robot>\n"; @@ -740,30 +740,30 @@ namespace VirtualRobot } // process manipulation objects - for (size_t i = 0; i < manipulationObjects.size(); i++) + for (auto & manipulationObject : manipulationObjects) { - ss << manipulationObjects[i]->toXML(basePath, 1, true); + ss << manipulationObject->toXML(basePath, 1, true); ss << "\n"; } // process obstacles - for (size_t i = 0; i < obstacles.size(); i++) + for (auto & obstacle : obstacles) { - ss << obstacles[i]->toXML(basePath, 1); + ss << obstacle->toXML(basePath, 1); ss << "\n"; } // process sceneObjectSets - for (size_t i = 0; i < sceneObjectSets.size(); i++) + for (auto & sceneObjectSet : sceneObjectSets) { - ss << sceneObjectSets[i]->toXML(1); + ss << sceneObjectSet->toXML(1); ss << "\n"; } // process trajectories - for (size_t i = 0; i < trajectories.size(); i++) + for (auto & trajectorie : trajectories) { - ss << trajectories[i]->toXML(1); + ss << trajectorie->toXML(1); ss << "\n"; } diff --git a/VirtualRobot/SceneObjectSet.cpp b/VirtualRobot/SceneObjectSet.cpp index 1b5dae76fc726989e1468d1f76133ad7947d0227..1ebbc663bf86db359f3bebdfe7e8e49e5ea17b7f 100644 --- a/VirtualRobot/SceneObjectSet.cpp +++ b/VirtualRobot/SceneObjectSet.cpp @@ -62,8 +62,8 @@ namespace VirtualRobot return false; } - for (unsigned i = 0; i < sceneObjects.size(); i++) - if (sceneObjects[i] == sceneObject) + for (const auto & i : sceneObjects) + if (i == sceneObject) { VR_WARNING << "col model already added, in: " << name << endl; return false; @@ -91,9 +91,9 @@ namespace VirtualRobot std::vector< SceneObjectPtr > so = sceneObjectSet->getSceneObjects(); - for (size_t i = 0; i < so.size(); i++) + for (const auto & i : so) { - if (!addSceneObject(so[i])) + if (!addSceneObject(i)) { return false; } @@ -116,15 +116,15 @@ namespace VirtualRobot bool SceneObjectSet::addSceneObjects(std::vector<RobotNodePtr> robotNodes) { - for (size_t i = 0; i < robotNodes.size(); i++) + for (auto & robotNode : robotNodes) { - SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNodes[i]); + SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNode); if (cm) { if (colChecker != cm->getCollisionChecker()) { - VR_WARNING << "col model of " << robotNodes[i]->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl; + VR_WARNING << "col model of " << robotNode->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl; } else { @@ -221,9 +221,9 @@ namespace VirtualRobot bool SceneObjectSet::getCurrentSceneObjectConfig(std::map< SceneObjectPtr, Eigen::Matrix4f >& storeConfig) { - for (std::vector< SceneObjectPtr >::iterator iter = sceneObjects.begin(); iter != sceneObjects.end(); iter++) + for (auto & sceneObject : sceneObjects) { - storeConfig[(*iter)] = (*iter)->getGlobalPose(); + storeConfig[sceneObject] = sceneObject->getGlobalPose(); } return true; @@ -231,9 +231,9 @@ namespace VirtualRobot bool SceneObjectSet::hasSceneObject(SceneObjectPtr sceneObject) { - for (std::vector< SceneObjectPtr >::iterator iter = sceneObjects.begin(); iter != sceneObjects.end(); iter++) + for (auto & iter : sceneObjects) { - if ((*iter) == sceneObject) + if (iter == sceneObject) { return true; } @@ -246,11 +246,11 @@ namespace VirtualRobot { std::vector< CollisionModelPtr > result; - for (size_t i = 0; i < sceneObjects.size(); i++) + for (auto & sceneObject : sceneObjects) { - if (sceneObjects[i]->getCollisionModel()) + if (sceneObject->getCollisionModel()) { - result.push_back(sceneObjects[i]->getCollisionModel()); + result.push_back(sceneObject->getCollisionModel()); } } @@ -286,9 +286,9 @@ namespace VirtualRobot ss << pre << "<SceneObjectSet name='" << name << "'>\n"; - for (size_t i = 0; i < sceneObjects.size(); i++) + for (auto & sceneObject : sceneObjects) { - ss << pre << t << "<SceneObject name='" << sceneObjects[i]->getName() << "'/>\n"; + ss << pre << t << "<SceneObject name='" << sceneObject->getName() << "'/>\n"; } ss << pre << "</SceneObjectSet>\n"; @@ -299,9 +299,9 @@ namespace VirtualRobot { SceneObjectSetPtr result(new SceneObjectSet(newName, colChecker)); - for (size_t i = 0; i < sceneObjects.size(); i++) + for (const auto & sceneObject : sceneObjects) { - result->addSceneObject(sceneObjects[i]); + result->addSceneObject(sceneObject); } return result; @@ -312,9 +312,9 @@ namespace VirtualRobot { SceneObjectSetPtr result(new SceneObjectSet(newName, newColChecker)); - for (size_t i = 0; i < sceneObjects.size(); i++) + for (auto & sceneObject : sceneObjects) { - SceneObjectPtr o = sceneObjects[i]->clone(sceneObjects[i]->getName(), newColChecker); + SceneObjectPtr o = sceneObject->clone(sceneObject->getName(), newColChecker); result->addSceneObject(o); } @@ -327,16 +327,16 @@ namespace VirtualRobot std::vector<VisualizationNodePtr> visus; std::vector<CollisionModelPtr> cols; - for (size_t i = 0; i < sceneObjects.size(); i++) + for (auto & sceneObject : sceneObjects) { - if (sceneObjects[i]->getVisualization()) + if (sceneObject->getVisualization()) { - visus.push_back(sceneObjects[i]->getVisualization()); + visus.push_back(sceneObject->getVisualization()); } - if (sceneObjects[i]->getCollisionModel()) + if (sceneObject->getCollisionModel()) { - cols.push_back(sceneObjects[i]->getCollisionModel()); + cols.push_back(sceneObject->getCollisionModel()); } } diff --git a/VirtualRobot/SphereApproximator.cpp b/VirtualRobot/SphereApproximator.cpp index 1a4a7c1b314a547d11ac2c03a437be4715292b17..7abf9004976ca26fb75cbc0d3745d49c3793f901 100644 --- a/VirtualRobot/SphereApproximator.cpp +++ b/VirtualRobot/SphereApproximator.cpp @@ -8,7 +8,7 @@ // ***************************************************************** #include "SphereApproximator.h" -#include <math.h> +#include <cmath> #include <iostream> #include "VirtualRobot/Visualization/TriMeshModel.h" @@ -122,9 +122,9 @@ namespace VirtualRobot setVec(v[2], -fSqrt3, fSqrt3, -fSqrt3); setVec(v[3], fSqrt3, -fSqrt3, -fSqrt3); - for (int i = 0 ; i < 4 ; i++) + for (const auto & i : v) { - gd.vertices.push_back(v[i]); + gd.vertices.push_back(i); } // structure describing a tetrahedron @@ -134,12 +134,12 @@ namespace VirtualRobot f[2].set(3, 2, 4); f[3].set(4, 1, 3); - for (int i = 0 ; i < 4 ; i++) + for (auto & i : f) { - f[i].id1--; - f[i].id2--; - f[i].id3--; - gd.faces.push_back(f[i]); + i.id1--; + i.id2--; + i.id3--; + gd.faces.push_back(i); } } @@ -154,9 +154,9 @@ namespace VirtualRobot setVec(v[4], 0, 0, 1); setVec(v[5], 0, 0, -1); - for (int i = 0 ; i < 6 ; i++) + for (const auto & i : v) { - gd.vertices.push_back(v[i]); + gd.vertices.push_back(i); } // Join vertices to create a unit octahedron @@ -170,12 +170,12 @@ namespace VirtualRobot f[6].set(2, 4, 6); f[7].set(4, 1, 6); - for (int i = 0 ; i < 8 ; i++) + for (auto & i : f) { - f[i].id1--; - f[i].id2--; - f[i].id3--; - gd.faces.push_back(f[i]); + i.id1--; + i.id2--; + i.id3--; + gd.faces.push_back(i); } } @@ -200,9 +200,9 @@ namespace VirtualRobot setVec(v[10], 0, -tau, -one); setVec(v[11], 0, tau, -one); - for (int i = 0 ; i < 12 ; i++) + for (const auto & i : v) { - gd.vertices.push_back(v[i]); + gd.vertices.push_back(i); } @@ -229,12 +229,12 @@ namespace VirtualRobot f[18].set(8, 10, 3); f[19].set(7, 3, 11); - for (int i = 0 ; i < 20 ; i++) + for (auto & i : f) { - f[i].id1--; - f[i].id2--; - f[i].id3--; - gd.faces.push_back(f[i]); + i.id1--; + i.id2--; + i.id3--; + gd.faces.push_back(i); } } @@ -319,9 +319,9 @@ namespace VirtualRobot fa[2].set(nC_m, nB_m, nC); fa[3].set(nA_m, nB_m, nC_m); - for (int i = 0 ; i < 4 ; i++) + for (const auto & i : fa) { - newFaces.push_back(fa[i]); + newFaces.push_back(i); } } diff --git a/VirtualRobot/TimeOptimalTrajectory/Path.cpp b/VirtualRobot/TimeOptimalTrajectory/Path.cpp index d6398d49630e176d239dd6f13c0a915b3483301b..fe51f30425a256a40943b5cca2baa67f646b50f6 100644 --- a/VirtualRobot/TimeOptimalTrajectory/Path.cpp +++ b/VirtualRobot/TimeOptimalTrajectory/Path.cpp @@ -205,13 +205,13 @@ namespace VirtualRobot } // create list of switching point candidates, calculate total path length and absolute positions of path segments - for(list<PathSegment*>::iterator segment = pathSegments.begin(); segment != pathSegments.end(); segment++) { - (*segment)->position = length; - list<double> localSwitchingPoints = (*segment)->getSwitchingPoints(); + for(auto & pathSegment : pathSegments) { + pathSegment->position = length; + list<double> localSwitchingPoints = pathSegment->getSwitchingPoints(); for(list<double>::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); point++) { switchingPoints.push_back(make_pair(length + *point, false)); } - length += (*segment)->getLength(); + length += pathSegment->getLength(); while(!switchingPoints.empty() && switchingPoints.back().first >= length) switchingPoints.pop_back(); switchingPoints.push_back(make_pair(length, true)); @@ -223,14 +223,14 @@ namespace VirtualRobot length(path.length), switchingPoints(path.switchingPoints) { - for(list<PathSegment*>::const_iterator it = path.pathSegments.begin(); it != path.pathSegments.end(); it++) { - pathSegments.push_back((*it)->clone()); + for(auto pathSegment : path.pathSegments) { + pathSegments.push_back(pathSegment->clone()); } } Path::~Path() { - for(list<PathSegment*>::iterator it = pathSegments.begin(); it != pathSegments.end(); it++) { - delete *it; + for(auto & pathSegment : pathSegments) { + delete pathSegment; } } diff --git a/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp b/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp index 9ed6c1ae841690d883d25e33147bfd7b81e64fe3..bfbd33daa8691eb7bf731c786faa1d9869047bbe 100644 --- a/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp +++ b/VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.cpp @@ -91,8 +91,7 @@ namespace VirtualRobot } } - TimeOptimalTrajectory::~TimeOptimalTrajectory(void) { - } + TimeOptimalTrajectory::~TimeOptimalTrajectory() = default; void TimeOptimalTrajectory::outputPhasePlaneTrajectory() const { ofstream file1("maxVelocity.txt"); @@ -106,11 +105,11 @@ namespace VirtualRobot file1.close(); ofstream file2("trajectory.txt"); - for(list<TimeOptimalTrajectoryStep>::const_iterator it = trajectory.begin(); it != trajectory.end(); it++) { - file2 << it->pathPos << " " << it->pathVel << endl; + for(const auto & it : trajectory) { + file2 << it.pathPos << " " << it.pathVel << endl; } - for(list<TimeOptimalTrajectoryStep>::const_iterator it = endTrajectory.begin(); it != endTrajectory.end(); it++) { - file2 << it->pathPos << " " << it->pathVel << endl; + for(const auto & it : endTrajectory) { + file2 << it.pathPos << " " << it.pathVel << endl; } file2.close(); } diff --git a/VirtualRobot/Tools/Gravity.cpp b/VirtualRobot/Tools/Gravity.cpp index 28e83045d3f8f0c8cb236bfb8bbf56e54ec3cd4d..bec1daae7ffdbfcc008089bd2432ff59a43d92f5 100644 --- a/VirtualRobot/Tools/Gravity.cpp +++ b/VirtualRobot/Tools/Gravity.cpp @@ -33,7 +33,7 @@ Gravity::Gravity(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr rns { if (body->getMass() <= 0) { - THROW_VR_EXCEPTION("No mass for body" << body->getName()); + THROW_VR_EXCEPTION("No mass for body '" << body->getName() << "' mass: " << body->getMass()); } } @@ -41,8 +41,7 @@ Gravity::Gravity(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr rns } Gravity::~Gravity() -{ -} += default; @@ -107,12 +106,7 @@ std::map<std::string, float> Gravity::computeGravityTorque() } Gravity::GravityData::GravityData() -{ - - - - -} += default; Gravity::GravityDataPtr Gravity::GravityData::create(SceneObjectPtr node, const std::vector<RobotNodePtr> &joints, const std::vector<RobotNodePtr> &bodies, std::vector<Gravity::GravityDataPtr> &dataVec) { @@ -134,8 +128,8 @@ void Gravity::GravityData::init(SceneObjectPtr node, const std::vector<RobotNode } } - for (size_t i = 0; i < bodies.size(); ++i) { - if(thisNode == bodies.at(i)){ + for (const auto & bodie : bodies) { + if(thisNode == bodie){ computeCoM = true; // VR_INFO << "Computing com for " << thisNode->getName() << std::endl; } diff --git a/VirtualRobot/Trajectory.cpp b/VirtualRobot/Trajectory.cpp index ff632f9ecaeeb5640063d16b49d5887d0b373418..976896e517af646bc6679796e109312effb181b0 100644 --- a/VirtualRobot/Trajectory.cpp +++ b/VirtualRobot/Trajectory.cpp @@ -252,7 +252,7 @@ namespace VirtualRobot { storePathPos = getPoint(0); - if (storeIndex != NULL) + if (storeIndex != nullptr) { *storeIndex = 0; } @@ -263,7 +263,7 @@ namespace VirtualRobot { storePathPos = getPoint(getNrOfPoints() - 1); - if (storeIndex != NULL) + if (storeIndex != nullptr) { *storeIndex = (int)path.size() - 1; } @@ -312,7 +312,7 @@ namespace VirtualRobot storePathPos = c1 + (c2 - c1) * factor; // storePos = startPos + factor*segment - if (storeIndex != NULL) + if (storeIndex != nullptr) { *storeIndex = startIndex; } diff --git a/VirtualRobot/Util/json/LICENSE.MIT b/VirtualRobot/Util/json/LICENSE.MIT new file mode 100644 index 0000000000000000000000000000000000000000..8b0f7002e40dd5015d793628b735bcd52a5579a0 --- /dev/null +++ b/VirtualRobot/Util/json/LICENSE.MIT @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2013-2018 Niels Lohmann + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/VirtualRobot/Util/json/eigen_conversion.hpp b/VirtualRobot/Util/json/eigen_conversion.hpp new file mode 100644 index 0000000000000000000000000000000000000000..65f3073d707eb46de459c7d8f2614bdef35eae56 --- /dev/null +++ b/VirtualRobot/Util/json/eigen_conversion.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include <type_traits> +#include <iostream> +#include <typeinfo> + +#include <Eigen/Core> + +#include "json.hpp" + +/** + * Provide to_json() and from_json() overloads for nlohmann::json, + * which allows simple syntax like: + * + * @code + * Eigen::Matrix3f in, out; + * + * json j; + * j = in; + * out = j; + * @endcode + * + * @test VirtualRobotJsonEigenConversionTest + * + * @see https://github.com/nlohmann/json#arbitrary-types-conversions + */ +namespace Eigen +{ + + // MatrixBase + + template <typename Derived> + void to_json(nlohmann::json& j, const MatrixBase<Derived>& matrix); + + template <typename Derived> + void from_json(const nlohmann::json& j, MatrixBase<Derived>& matrix); + + + // Specialization for Vector3f + + template <> + void to_json<Vector3f>(nlohmann::json& j, const MatrixBase<Vector3f>& vector); + + template <> + void from_json<Vector3f>(const nlohmann::json& j, MatrixBase<Vector3f>& vector); + + + // Quaternion + + template <typename Derived> + void to_json(nlohmann::json& j, const QuaternionBase<Derived>& quat); + + template <typename Derived> + void from_json(const nlohmann::json& j, QuaternionBase<Derived>& quat); + + + + // IMPLEMENTATION + + template <typename Derived> + void to_json(nlohmann::json& j, const MatrixBase<Derived>& matrix) + { + for (int row = 0; row < matrix.rows(); ++row) + { + nlohmann::json column = nlohmann::json::array(); + for (int col = 0; col < matrix.cols(); ++col) + { + column.push_back(matrix(row, col)); + } + j.push_back(column); + } + } + + template <typename Derived> + void from_json(const nlohmann::json& j, MatrixBase<Derived>& matrix) + { + using Scalar = typename MatrixBase<Derived>::Scalar; + + for (std::size_t row = 0; row < j.size(); ++row) + { + const auto& jrow = j.at(row); + for (std::size_t col = 0; col < jrow.size(); ++col) + { + const auto& value = jrow.at(col); + matrix(row, col) = value.get<Scalar>(); + } + } + } + + + template <> + void to_json<Vector3f>(nlohmann::json& j, const MatrixBase<Vector3f>& vector) + { + j["x"] = vector.x(); + j["y"] = vector.y(); + j["z"] = vector.z(); + } + + template <> + void from_json<Vector3f>(const nlohmann::json& j, MatrixBase<Vector3f>& vector) + { + vector.x() = j.at("x").get<float>(); + vector.y() = j.at("y").get<float>(); + vector.z() = j.at("z").get<float>(); + } + + + template <typename Derived> + void to_json(nlohmann::json& j, const QuaternionBase<Derived>& quat) + { + j["qw"] = quat.w(); + j["qx"] = quat.x(); + j["qy"] = quat.y(); + j["qz"] = quat.z(); + } + + template <typename Derived> + void from_json(const nlohmann::json& j, QuaternionBase<Derived>& quat) + { + using Scalar = typename QuaternionBase<Derived>::Scalar; + quat.w() = j.at("qw").get<Scalar>(); + quat.x() = j.at("qx").get<Scalar>(); + quat.y() = j.at("qy").get<Scalar>(); + quat.z() = j.at("qz").get<Scalar>(); + } + +} diff --git a/VirtualRobot/Util/json/json.hpp b/VirtualRobot/Util/json/json.hpp new file mode 100644 index 0000000000000000000000000000000000000000..931e7b3662061d92aacc45f018307bae979af73c --- /dev/null +++ b/VirtualRobot/Util/json/json.hpp @@ -0,0 +1,20277 @@ +/* + __ _____ _____ _____ + __| | __| | | | JSON for Modern C++ +| | |__ | | | | | | version 3.4.0 +|_____|_____|_____|_|___| https://github.com/nlohmann/json + +Licensed under the MIT License <http://opensource.org/licenses/MIT>. +SPDX-License-Identifier: MIT +Copyright (c) 2013-2018 Niels Lohmann <http://nlohmann.me>. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#ifndef NLOHMANN_JSON_HPP +#define NLOHMANN_JSON_HPP + +#define NLOHMANN_JSON_VERSION_MAJOR 3 +#define NLOHMANN_JSON_VERSION_MINOR 4 +#define NLOHMANN_JSON_VERSION_PATCH 0 + +#include <algorithm> // all_of, find, for_each +#include <cassert> // assert +#include <ciso646> // and, not, or +#include <cstddef> // nullptr_t, ptrdiff_t, size_t +#include <functional> // hash, less +#include <initializer_list> // initializer_list +#include <iosfwd> // istream, ostream +#include <iterator> // iterator_traits, random_access_iterator_tag +#include <numeric> // accumulate +#include <string> // string, stoi, to_string +#include <utility> // declval, forward, move, pair, swap + +// #include <nlohmann/json_fwd.hpp> +#ifndef NLOHMANN_JSON_FWD_HPP +#define NLOHMANN_JSON_FWD_HPP + +#include <cstdint> // int64_t, uint64_t +#include <map> // map +#include <memory> // allocator +#include <string> // string +#include <vector> // vector + +/*! +@brief namespace for Niels Lohmann +@see https://github.com/nlohmann +@since version 1.0.0 +*/ +namespace nlohmann +{ +/*! +@brief default JSONSerializer template argument + +This serializer ignores the template arguments and uses ADL +([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl)) +for serialization. +*/ +template<typename T = void, typename SFINAE = void> +struct adl_serializer; + +template<template<typename U, typename V, typename... Args> class ObjectType = + std::map, + template<typename U, typename... Args> class ArrayType = std::vector, + class StringType = std::string, class BooleanType = bool, + class NumberIntegerType = std::int64_t, + class NumberUnsignedType = std::uint64_t, + class NumberFloatType = double, + template<typename U> class AllocatorType = std::allocator, + template<typename T, typename SFINAE = void> class JSONSerializer = + adl_serializer> +class basic_json; + +/*! +@brief JSON Pointer + +A JSON pointer defines a string syntax for identifying a specific value +within a JSON document. It can be used with functions `at` and +`operator[]`. Furthermore, JSON pointers are the base for JSON patches. + +@sa [RFC 6901](https://tools.ietf.org/html/rfc6901) + +@since version 2.0.0 +*/ +template<typename BasicJsonType> +class json_pointer; + +/*! +@brief default JSON class + +This type is the default specialization of the @ref basic_json class which +uses the standard template types. + +@since version 1.0.0 +*/ +using json = basic_json<>; +} // namespace nlohmann + +#endif + +// #include <nlohmann/detail/macro_scope.hpp> + + +// This file contains all internal macro definitions +// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them + +// exclude unsupported compilers +#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK) + #if defined(__clang__) + #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400 + #error "unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER)) + #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800 + #error "unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #endif +#endif + +// disable float-equal warnings on GCC/clang +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + +// disable documentation warnings on clang +#if defined(__clang__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdocumentation" +#endif + +// allow for portable deprecation warnings +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #define JSON_DEPRECATED __attribute__((deprecated)) +#elif defined(_MSC_VER) + #define JSON_DEPRECATED __declspec(deprecated) +#else + #define JSON_DEPRECATED +#endif + +// allow to disable exceptions +#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)) && !defined(JSON_NOEXCEPTION) + #define JSON_THROW(exception) throw exception + #define JSON_TRY try + #define JSON_CATCH(exception) catch(exception) + #define JSON_INTERNAL_CATCH(exception) catch(exception) +#else + #define JSON_THROW(exception) std::abort() + #define JSON_TRY if(true) + #define JSON_CATCH(exception) if(false) + #define JSON_INTERNAL_CATCH(exception) if(false) +#endif + +// override exception macros +#if defined(JSON_THROW_USER) + #undef JSON_THROW + #define JSON_THROW JSON_THROW_USER +#endif +#if defined(JSON_TRY_USER) + #undef JSON_TRY + #define JSON_TRY JSON_TRY_USER +#endif +#if defined(JSON_CATCH_USER) + #undef JSON_CATCH + #define JSON_CATCH JSON_CATCH_USER + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_CATCH_USER +#endif +#if defined(JSON_INTERNAL_CATCH_USER) + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_INTERNAL_CATCH_USER +#endif + +// manual branch prediction +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #define JSON_LIKELY(x) __builtin_expect(!!(x), 1) + #define JSON_UNLIKELY(x) __builtin_expect(!!(x), 0) +#else + #define JSON_LIKELY(x) x + #define JSON_UNLIKELY(x) x +#endif + +// C++ language standard detection +#if (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 +#elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1) + #define JSON_HAS_CPP_14 +#endif + +/*! +@brief macro to briefly define a mapping between an enum and JSON +@def NLOHMANN_JSON_SERIALIZE_ENUM +@since version 3.4.0 +*/ +#define NLOHMANN_JSON_SERIALIZE_ENUM(ENUM_TYPE, ...) \ + template<typename BasicJsonType> \ + inline void to_json(BasicJsonType& j, const ENUM_TYPE& e) \ + { \ + static_assert(std::is_enum<ENUM_TYPE>::value, #ENUM_TYPE " must be an enum!"); \ + static const std::pair<ENUM_TYPE, BasicJsonType> m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [e](const std::pair<ENUM_TYPE, BasicJsonType>& ej_pair) -> bool \ + { \ + return ej_pair.first == e; \ + }); \ + j = ((it != std::end(m)) ? it : std::begin(m))->second; \ + } \ + template<typename BasicJsonType> \ + inline void from_json(const BasicJsonType& j, ENUM_TYPE& e) \ + { \ + static_assert(std::is_enum<ENUM_TYPE>::value, #ENUM_TYPE " must be an enum!"); \ + static const std::pair<ENUM_TYPE, BasicJsonType> m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [j](const std::pair<ENUM_TYPE, BasicJsonType>& ej_pair) -> bool \ + { \ + return ej_pair.second == j; \ + }); \ + e = ((it != std::end(m)) ? it : std::begin(m))->first; \ + } + +// Ugly macros to avoid uglier copy-paste when specializing basic_json. They +// may be removed in the future once the class is split. + +#define NLOHMANN_BASIC_JSON_TPL_DECLARATION \ + template<template<typename, typename, typename...> class ObjectType, \ + template<typename, typename...> class ArrayType, \ + class StringType, class BooleanType, class NumberIntegerType, \ + class NumberUnsignedType, class NumberFloatType, \ + template<typename> class AllocatorType, \ + template<typename, typename = void> class JSONSerializer> + +#define NLOHMANN_BASIC_JSON_TPL \ + basic_json<ObjectType, ArrayType, StringType, BooleanType, \ + NumberIntegerType, NumberUnsignedType, NumberFloatType, \ + AllocatorType, JSONSerializer> + +// #include <nlohmann/detail/meta/cpp_future.hpp> + + +#include <ciso646> // not +#include <cstddef> // size_t +#include <type_traits> // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type + +namespace nlohmann +{ +namespace detail +{ +// alias templates to reduce boilerplate +template<bool B, typename T = void> +using enable_if_t = typename std::enable_if<B, T>::type; + +template<typename T> +using uncvref_t = typename std::remove_cv<typename std::remove_reference<T>::type>::type; + +// implementation of C++14 index_sequence and affiliates +// source: https://stackoverflow.com/a/32223343 +template<std::size_t... Ints> +struct index_sequence +{ + using type = index_sequence; + using value_type = std::size_t; + static constexpr std::size_t size() noexcept + { + return sizeof...(Ints); + } +}; + +template<class Sequence1, class Sequence2> +struct merge_and_renumber; + +template<std::size_t... I1, std::size_t... I2> +struct merge_and_renumber<index_sequence<I1...>, index_sequence<I2...>> + : index_sequence < I1..., (sizeof...(I1) + I2)... > {}; + +template<std::size_t N> +struct make_index_sequence + : merge_and_renumber < typename make_index_sequence < N / 2 >::type, + typename make_index_sequence < N - N / 2 >::type > {}; + +template<> struct make_index_sequence<0> : index_sequence<> {}; +template<> struct make_index_sequence<1> : index_sequence<0> {}; + +template<typename... Ts> +using index_sequence_for = make_index_sequence<sizeof...(Ts)>; + +// dispatch utility (taken from ranges-v3) +template<unsigned N> struct priority_tag : priority_tag < N - 1 > {}; +template<> struct priority_tag<0> {}; + +// taken from ranges-v3 +template<typename T> +struct static_const +{ + static constexpr T value{}; +}; + +template<typename T> +constexpr T static_const<T>::value; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/meta/type_traits.hpp> + + +#include <ciso646> // not +#include <limits> // numeric_limits +#include <type_traits> // false_type, is_constructible, is_integral, is_same, true_type +#include <utility> // declval + +// #include <nlohmann/json_fwd.hpp> + +// #include <nlohmann/detail/meta/cpp_future.hpp> + +// #include <nlohmann/detail/meta/detected.hpp> + + +#include <type_traits> + +// #include <nlohmann/detail/meta/void_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +template <typename ...Ts> struct make_void +{ + using type = void; +}; +template <typename ...Ts> using void_t = typename make_void<Ts...>::type; +} // namespace detail +} // namespace nlohmann + + +// http://en.cppreference.com/w/cpp/experimental/is_detected +namespace nlohmann +{ +namespace detail +{ +struct nonesuch +{ + nonesuch() = delete; + ~nonesuch() = delete; + nonesuch(nonesuch const&) = delete; + void operator=(nonesuch const&) = delete; +}; + +template <class Default, + class AlwaysVoid, + template <class...> class Op, + class... Args> +struct detector +{ + using value_t = std::false_type; + using type = Default; +}; + +template <class Default, template <class...> class Op, class... Args> +struct detector<Default, void_t<Op<Args...>>, Op, Args...> +{ + using value_t = std::true_type; + using type = Op<Args...>; +}; + +template <template <class...> class Op, class... Args> +using is_detected = typename detector<nonesuch, void, Op, Args...>::value_t; + +template <template <class...> class Op, class... Args> +using detected_t = typename detector<nonesuch, void, Op, Args...>::type; + +template <class Default, template <class...> class Op, class... Args> +using detected_or = detector<Default, void, Op, Args...>; + +template <class Default, template <class...> class Op, class... Args> +using detected_or_t = typename detected_or<Default, Op, Args...>::type; + +template <class Expected, template <class...> class Op, class... Args> +using is_detected_exact = std::is_same<Expected, detected_t<Op, Args...>>; + +template <class To, template <class...> class Op, class... Args> +using is_detected_convertible = + std::is_convertible<detected_t<Op, Args...>, To>; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/macro_scope.hpp> + + +namespace nlohmann +{ +/*! +@brief detail namespace with internal helper functions + +This namespace collects functions that should not be exposed, +implementations of some @ref basic_json methods, and meta-programming helpers. + +@since version 2.1.0 +*/ +namespace detail +{ +///////////// +// helpers // +///////////// + +// Note to maintainers: +// +// Every trait in this file expects a non CV-qualified type. +// The only exceptions are in the 'aliases for detected' section +// (i.e. those of the form: decltype(T::member_function(std::declval<T>()))) +// +// In this case, T has to be properly CV-qualified to constraint the function arguments +// (e.g. to_json(BasicJsonType&, const T&)) + +template<typename> struct is_basic_json : std::false_type {}; + +NLOHMANN_BASIC_JSON_TPL_DECLARATION +struct is_basic_json<NLOHMANN_BASIC_JSON_TPL> : std::true_type {}; + +////////////////////////// +// aliases for detected // +////////////////////////// + +template <typename T> +using mapped_type_t = typename T::mapped_type; + +template <typename T> +using key_type_t = typename T::key_type; + +template <typename T> +using value_type_t = typename T::value_type; + +template <typename T> +using difference_type_t = typename T::difference_type; + +template <typename T> +using pointer_t = typename T::pointer; + +template <typename T> +using reference_t = typename T::reference; + +template <typename T> +using iterator_category_t = typename T::iterator_category; + +template <typename T> +using iterator_t = typename T::iterator; + +template <typename T, typename... Args> +using to_json_function = decltype(T::to_json(std::declval<Args>()...)); + +template <typename T, typename... Args> +using from_json_function = decltype(T::from_json(std::declval<Args>()...)); + +template <typename T, typename U> +using get_template_function = decltype(std::declval<T>().template get<U>()); + +// trait checking if JSONSerializer<T>::from_json(json const&, udt&) exists +template <typename BasicJsonType, typename T, typename = void> +struct has_from_json : std::false_type {}; + +template <typename BasicJsonType, typename T> +struct has_from_json<BasicJsonType, T, + enable_if_t<not is_basic_json<T>::value>> +{ + using serializer = typename BasicJsonType::template json_serializer<T, void>; + + static constexpr bool value = + is_detected_exact<void, from_json_function, serializer, + const BasicJsonType&, T&>::value; +}; + +// This trait checks if JSONSerializer<T>::from_json(json const&) exists +// this overload is used for non-default-constructible user-defined-types +template <typename BasicJsonType, typename T, typename = void> +struct has_non_default_from_json : std::false_type {}; + +template<typename BasicJsonType, typename T> +struct has_non_default_from_json<BasicJsonType, T, enable_if_t<not is_basic_json<T>::value>> +{ + using serializer = typename BasicJsonType::template json_serializer<T, void>; + + static constexpr bool value = + is_detected_exact<T, from_json_function, serializer, + const BasicJsonType&>::value; +}; + +// This trait checks if BasicJsonType::json_serializer<T>::to_json exists +// Do not evaluate the trait when T is a basic_json type, to avoid template instantiation infinite recursion. +template <typename BasicJsonType, typename T, typename = void> +struct has_to_json : std::false_type {}; + +template <typename BasicJsonType, typename T> +struct has_to_json<BasicJsonType, T, enable_if_t<not is_basic_json<T>::value>> +{ + using serializer = typename BasicJsonType::template json_serializer<T, void>; + + static constexpr bool value = + is_detected_exact<void, to_json_function, serializer, BasicJsonType&, + T>::value; +}; + + +/////////////////// +// is_ functions // +/////////////////// + +template <typename T, typename = void> +struct is_iterator_traits : std::false_type {}; + +template <typename T> +struct is_iterator_traits<std::iterator_traits<T>> +{ + private: + using traits = std::iterator_traits<T>; + + public: + static constexpr auto value = + is_detected<value_type_t, traits>::value && + is_detected<difference_type_t, traits>::value && + is_detected<pointer_t, traits>::value && + is_detected<iterator_category_t, traits>::value && + is_detected<reference_t, traits>::value; +}; + +// source: https://stackoverflow.com/a/37193089/4116453 + +template <typename T, typename = void> +struct is_complete_type : std::false_type {}; + +template <typename T> +struct is_complete_type<T, decltype(void(sizeof(T)))> : std::true_type {}; + +template <typename BasicJsonType, typename CompatibleObjectType, + typename = void> +struct is_compatible_object_type_impl : std::false_type {}; + +template <typename BasicJsonType, typename CompatibleObjectType> +struct is_compatible_object_type_impl < + BasicJsonType, CompatibleObjectType, + enable_if_t<is_detected<mapped_type_t, CompatibleObjectType>::value and + is_detected<key_type_t, CompatibleObjectType>::value >> +{ + + using object_t = typename BasicJsonType::object_t; + + // macOS's is_constructible does not play well with nonesuch... + static constexpr bool value = + std::is_constructible<typename object_t::key_type, + typename CompatibleObjectType::key_type>::value and + std::is_constructible<typename object_t::mapped_type, + typename CompatibleObjectType::mapped_type>::value; +}; + +template <typename BasicJsonType, typename CompatibleObjectType> +struct is_compatible_object_type + : is_compatible_object_type_impl<BasicJsonType, CompatibleObjectType> {}; + +template <typename BasicJsonType, typename ConstructibleObjectType, + typename = void> +struct is_constructible_object_type_impl : std::false_type {}; + +template <typename BasicJsonType, typename ConstructibleObjectType> +struct is_constructible_object_type_impl < + BasicJsonType, ConstructibleObjectType, + enable_if_t<is_detected<mapped_type_t, ConstructibleObjectType>::value and + is_detected<key_type_t, ConstructibleObjectType>::value >> +{ + using object_t = typename BasicJsonType::object_t; + + static constexpr bool value = + (std::is_constructible<typename ConstructibleObjectType::key_type, typename object_t::key_type>::value and + std::is_same<typename object_t::mapped_type, typename ConstructibleObjectType::mapped_type>::value) or + (has_from_json<BasicJsonType, typename ConstructibleObjectType::mapped_type>::value or + has_non_default_from_json<BasicJsonType, typename ConstructibleObjectType::mapped_type >::value); +}; + +template <typename BasicJsonType, typename ConstructibleObjectType> +struct is_constructible_object_type + : is_constructible_object_type_impl<BasicJsonType, + ConstructibleObjectType> {}; + +template <typename BasicJsonType, typename CompatibleStringType, + typename = void> +struct is_compatible_string_type_impl : std::false_type {}; + +template <typename BasicJsonType, typename CompatibleStringType> +struct is_compatible_string_type_impl < + BasicJsonType, CompatibleStringType, + enable_if_t<is_detected_exact<typename BasicJsonType::string_t::value_type, + value_type_t, CompatibleStringType>::value >> +{ + static constexpr auto value = + std::is_constructible<typename BasicJsonType::string_t, CompatibleStringType>::value; +}; + +template <typename BasicJsonType, typename ConstructibleStringType> +struct is_compatible_string_type + : is_compatible_string_type_impl<BasicJsonType, ConstructibleStringType> {}; + +template <typename BasicJsonType, typename ConstructibleStringType, + typename = void> +struct is_constructible_string_type_impl : std::false_type {}; + +template <typename BasicJsonType, typename ConstructibleStringType> +struct is_constructible_string_type_impl < + BasicJsonType, ConstructibleStringType, + enable_if_t<is_detected_exact<typename BasicJsonType::string_t::value_type, + value_type_t, ConstructibleStringType>::value >> +{ + static constexpr auto value = + std::is_constructible<ConstructibleStringType, + typename BasicJsonType::string_t>::value; +}; + +template <typename BasicJsonType, typename ConstructibleStringType> +struct is_constructible_string_type + : is_constructible_string_type_impl<BasicJsonType, ConstructibleStringType> {}; + +template <typename BasicJsonType, typename CompatibleArrayType, typename = void> +struct is_compatible_array_type_impl : std::false_type {}; + +template <typename BasicJsonType, typename CompatibleArrayType> +struct is_compatible_array_type_impl < + BasicJsonType, CompatibleArrayType, + enable_if_t<is_detected<value_type_t, CompatibleArrayType>::value and + is_detected<iterator_t, CompatibleArrayType>::value and +// This is needed because json_reverse_iterator has a ::iterator type... +// Therefore it is detected as a CompatibleArrayType. +// The real fix would be to have an Iterable concept. + not is_iterator_traits< + std::iterator_traits<CompatibleArrayType>>::value >> +{ + static constexpr bool value = + std::is_constructible<BasicJsonType, + typename CompatibleArrayType::value_type>::value; +}; + +template <typename BasicJsonType, typename CompatibleArrayType> +struct is_compatible_array_type + : is_compatible_array_type_impl<BasicJsonType, CompatibleArrayType> {}; + +template <typename BasicJsonType, typename ConstructibleArrayType, typename = void> +struct is_constructible_array_type_impl : std::false_type {}; + +template <typename BasicJsonType, typename ConstructibleArrayType> +struct is_constructible_array_type_impl < + BasicJsonType, ConstructibleArrayType, + enable_if_t<std::is_same<ConstructibleArrayType, + typename BasicJsonType::value_type>::value >> + : std::true_type {}; + +template <typename BasicJsonType, typename ConstructibleArrayType> +struct is_constructible_array_type_impl < + BasicJsonType, ConstructibleArrayType, + enable_if_t<not std::is_same<ConstructibleArrayType, + typename BasicJsonType::value_type>::value and + is_detected<value_type_t, ConstructibleArrayType>::value and + is_detected<iterator_t, ConstructibleArrayType>::value and + is_complete_type< + detected_t<value_type_t, ConstructibleArrayType>>::value >> +{ + static constexpr bool value = + // This is needed because json_reverse_iterator has a ::iterator type, + // furthermore, std::back_insert_iterator (and other iterators) have a base class `iterator`... + // Therefore it is detected as a ConstructibleArrayType. + // The real fix would be to have an Iterable concept. + not is_iterator_traits < + std::iterator_traits<ConstructibleArrayType >>::value and + + (std::is_same<typename ConstructibleArrayType::value_type, typename BasicJsonType::array_t::value_type>::value or + has_from_json<BasicJsonType, + typename ConstructibleArrayType::value_type>::value or + has_non_default_from_json < + BasicJsonType, typename ConstructibleArrayType::value_type >::value); +}; + +template <typename BasicJsonType, typename ConstructibleArrayType> +struct is_constructible_array_type + : is_constructible_array_type_impl<BasicJsonType, ConstructibleArrayType> {}; + +template <typename RealIntegerType, typename CompatibleNumberIntegerType, + typename = void> +struct is_compatible_integer_type_impl : std::false_type {}; + +template <typename RealIntegerType, typename CompatibleNumberIntegerType> +struct is_compatible_integer_type_impl < + RealIntegerType, CompatibleNumberIntegerType, + enable_if_t<std::is_integral<RealIntegerType>::value and + std::is_integral<CompatibleNumberIntegerType>::value and + not std::is_same<bool, CompatibleNumberIntegerType>::value >> +{ + // is there an assert somewhere on overflows? + using RealLimits = std::numeric_limits<RealIntegerType>; + using CompatibleLimits = std::numeric_limits<CompatibleNumberIntegerType>; + + static constexpr auto value = + std::is_constructible<RealIntegerType, + CompatibleNumberIntegerType>::value and + CompatibleLimits::is_integer and + RealLimits::is_signed == CompatibleLimits::is_signed; +}; + +template <typename RealIntegerType, typename CompatibleNumberIntegerType> +struct is_compatible_integer_type + : is_compatible_integer_type_impl<RealIntegerType, + CompatibleNumberIntegerType> {}; + +template <typename BasicJsonType, typename CompatibleType, typename = void> +struct is_compatible_type_impl: std::false_type {}; + +template <typename BasicJsonType, typename CompatibleType> +struct is_compatible_type_impl < + BasicJsonType, CompatibleType, + enable_if_t<is_complete_type<CompatibleType>::value >> +{ + static constexpr bool value = + has_to_json<BasicJsonType, CompatibleType>::value; +}; + +template <typename BasicJsonType, typename CompatibleType> +struct is_compatible_type + : is_compatible_type_impl<BasicJsonType, CompatibleType> {}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/exceptions.hpp> + + +#include <exception> // exception +#include <stdexcept> // runtime_error +#include <string> // to_string + +// #include <nlohmann/detail/input/position_t.hpp> + + +#include <cstddef> // size_t + +namespace nlohmann +{ +namespace detail +{ +/// struct to capture the start position of the current token +struct position_t +{ + /// the total number of characters read + std::size_t chars_read_total = 0; + /// the number of characters read in the current line + std::size_t chars_read_current_line = 0; + /// the number of lines read + std::size_t lines_read = 0; + + /// conversion to size_t to preserve SAX interface + constexpr operator size_t() const + { + return chars_read_total; + } +}; + +} +} + + +namespace nlohmann +{ +namespace detail +{ +//////////////// +// exceptions // +//////////////// + +/*! +@brief general exception of the @ref basic_json class + +This class is an extension of `std::exception` objects with a member @a id for +exception ids. It is used as the base class for all exceptions thrown by the +@ref basic_json class. This class can hence be used as "wildcard" to catch +exceptions. + +Subclasses: +- @ref parse_error for exceptions indicating a parse error +- @ref invalid_iterator for exceptions indicating errors with iterators +- @ref type_error for exceptions indicating executing a member function with + a wrong type +- @ref out_of_range for exceptions indicating access out of the defined range +- @ref other_error for exceptions indicating other library errors + +@internal +@note To have nothrow-copy-constructible exceptions, we internally use + `std::runtime_error` which can cope with arbitrary-length error messages. + Intermediate strings are built with static functions and then passed to + the actual constructor. +@endinternal + +@liveexample{The following code shows how arbitrary library exceptions can be +caught.,exception} + +@since version 3.0.0 +*/ +class exception : public std::exception +{ + public: + /// returns the explanatory string + const char* what() const noexcept override + { + return m.what(); + } + + /// the id of the exception + const int id; + + protected: + exception(int id_, const char* what_arg) : id(id_), m(what_arg) {} + + static std::string name(const std::string& ename, int id_) + { + return "[json.exception." + ename + "." + std::to_string(id_) + "] "; + } + + private: + /// an exception object as storage for error messages + std::runtime_error m; +}; + +/*! +@brief exception indicating a parse error + +This exception is thrown by the library when a parse error occurs. Parse errors +can occur during the deserialization of JSON text, CBOR, MessagePack, as well +as when using JSON Patch. + +Member @a byte holds the byte index of the last read character in the input +file. + +Exceptions have ids 1xx. + +name / id | example message | description +------------------------------ | --------------- | ------------------------- +json.exception.parse_error.101 | parse error at 2: unexpected end of input; expected string literal | This error indicates a syntax error while deserializing a JSON text. The error message describes that an unexpected token (character) was encountered, and the member @a byte indicates the error position. +json.exception.parse_error.102 | parse error at 14: missing or wrong low surrogate | JSON uses the `\uxxxx` format to describe Unicode characters. Code points above above 0xFFFF are split into two `\uxxxx` entries ("surrogate pairs"). This error indicates that the surrogate pair is incomplete or contains an invalid code point. +json.exception.parse_error.103 | parse error: code points above 0x10FFFF are invalid | Unicode supports code points up to 0x10FFFF. Code points above 0x10FFFF are invalid. +json.exception.parse_error.104 | parse error: JSON patch must be an array of objects | [RFC 6902](https://tools.ietf.org/html/rfc6902) requires a JSON Patch document to be a JSON document that represents an array of objects. +json.exception.parse_error.105 | parse error: operation must have string member 'op' | An operation of a JSON Patch document must contain exactly one "op" member, whose value indicates the operation to perform. Its value must be one of "add", "remove", "replace", "move", "copy", or "test"; other values are errors. +json.exception.parse_error.106 | parse error: array index '01' must not begin with '0' | An array index in a JSON Pointer ([RFC 6901](https://tools.ietf.org/html/rfc6901)) may be `0` or any number without a leading `0`. +json.exception.parse_error.107 | parse error: JSON pointer must be empty or begin with '/' - was: 'foo' | A JSON Pointer must be a Unicode string containing a sequence of zero or more reference tokens, each prefixed by a `/` character. +json.exception.parse_error.108 | parse error: escape character '~' must be followed with '0' or '1' | In a JSON Pointer, only `~0` and `~1` are valid escape sequences. +json.exception.parse_error.109 | parse error: array index 'one' is not a number | A JSON Pointer array index must be a number. +json.exception.parse_error.110 | parse error at 1: cannot read 2 bytes from vector | When parsing CBOR or MessagePack, the byte vector ends before the complete value has been read. +json.exception.parse_error.112 | parse error at 1: error reading CBOR; last byte: 0xF8 | Not all types of CBOR or MessagePack are supported. This exception occurs if an unsupported byte was read. +json.exception.parse_error.113 | parse error at 2: expected a CBOR string; last byte: 0x98 | While parsing a map key, a value that is not a string has been read. +json.exception.parse_error.114 | parse error: Unsupported BSON record type 0x0F | The parsing of the corresponding BSON record type is not implemented (yet). + +@note For an input with n bytes, 1 is the index of the first character and n+1 + is the index of the terminating null byte or the end of file. This also + holds true when reading a byte vector (CBOR or MessagePack). + +@liveexample{The following code shows how a `parse_error` exception can be +caught.,parse_error} + +@sa @ref exception for the base class of the library exceptions +@sa @ref invalid_iterator for exceptions indicating errors with iterators +@sa @ref type_error for exceptions indicating executing a member function with + a wrong type +@sa @ref out_of_range for exceptions indicating access out of the defined range +@sa @ref other_error for exceptions indicating other library errors + +@since version 3.0.0 +*/ +class parse_error : public exception +{ + public: + /*! + @brief create a parse error exception + @param[in] id_ the id of the exception + @param[in] position the position where the error occurred (or with + chars_read_total=0 if the position cannot be + determined) + @param[in] what_arg the explanatory string + @return parse_error object + */ + static parse_error create(int id_, const position_t& pos, const std::string& what_arg) + { + std::string w = exception::name("parse_error", id_) + "parse error" + + position_string(pos) + ": " + what_arg; + return parse_error(id_, pos.chars_read_total, w.c_str()); + } + + static parse_error create(int id_, std::size_t byte_, const std::string& what_arg) + { + std::string w = exception::name("parse_error", id_) + "parse error" + + (byte_ != 0 ? (" at byte " + std::to_string(byte_)) : "") + + ": " + what_arg; + return parse_error(id_, byte_, w.c_str()); + } + + /*! + @brief byte index of the parse error + + The byte index of the last read character in the input file. + + @note For an input with n bytes, 1 is the index of the first character and + n+1 is the index of the terminating null byte or the end of file. + This also holds true when reading a byte vector (CBOR or MessagePack). + */ + const std::size_t byte; + + private: + parse_error(int id_, std::size_t byte_, const char* what_arg) + : exception(id_, what_arg), byte(byte_) {} + + static std::string position_string(const position_t& pos) + { + return " at line " + std::to_string(pos.lines_read + 1) + + ", column " + std::to_string(pos.chars_read_current_line); + } +}; + +/*! +@brief exception indicating errors with iterators + +This exception is thrown if iterators passed to a library function do not match +the expected semantics. + +Exceptions have ids 2xx. + +name / id | example message | description +----------------------------------- | --------------- | ------------------------- +json.exception.invalid_iterator.201 | iterators are not compatible | The iterators passed to constructor @ref basic_json(InputIT first, InputIT last) are not compatible, meaning they do not belong to the same container. Therefore, the range (@a first, @a last) is invalid. +json.exception.invalid_iterator.202 | iterator does not fit current value | In an erase or insert function, the passed iterator @a pos does not belong to the JSON value for which the function was called. It hence does not define a valid position for the deletion/insertion. +json.exception.invalid_iterator.203 | iterators do not fit current value | Either iterator passed to function @ref erase(IteratorType first, IteratorType last) does not belong to the JSON value from which values shall be erased. It hence does not define a valid range to delete values from. +json.exception.invalid_iterator.204 | iterators out of range | When an iterator range for a primitive type (number, boolean, or string) is passed to a constructor or an erase function, this range has to be exactly (@ref begin(), @ref end()), because this is the only way the single stored value is expressed. All other ranges are invalid. +json.exception.invalid_iterator.205 | iterator out of range | When an iterator for a primitive type (number, boolean, or string) is passed to an erase function, the iterator has to be the @ref begin() iterator, because it is the only way to address the stored value. All other iterators are invalid. +json.exception.invalid_iterator.206 | cannot construct with iterators from null | The iterators passed to constructor @ref basic_json(InputIT first, InputIT last) belong to a JSON null value and hence to not define a valid range. +json.exception.invalid_iterator.207 | cannot use key() for non-object iterators | The key() member function can only be used on iterators belonging to a JSON object, because other types do not have a concept of a key. +json.exception.invalid_iterator.208 | cannot use operator[] for object iterators | The operator[] to specify a concrete offset cannot be used on iterators belonging to a JSON object, because JSON objects are unordered. +json.exception.invalid_iterator.209 | cannot use offsets with object iterators | The offset operators (+, -, +=, -=) cannot be used on iterators belonging to a JSON object, because JSON objects are unordered. +json.exception.invalid_iterator.210 | iterators do not fit | The iterator range passed to the insert function are not compatible, meaning they do not belong to the same container. Therefore, the range (@a first, @a last) is invalid. +json.exception.invalid_iterator.211 | passed iterators may not belong to container | The iterator range passed to the insert function must not be a subrange of the container to insert to. +json.exception.invalid_iterator.212 | cannot compare iterators of different containers | When two iterators are compared, they must belong to the same container. +json.exception.invalid_iterator.213 | cannot compare order of object iterators | The order of object iterators cannot be compared, because JSON objects are unordered. +json.exception.invalid_iterator.214 | cannot get value | Cannot get value for iterator: Either the iterator belongs to a null value or it is an iterator to a primitive type (number, boolean, or string), but the iterator is different to @ref begin(). + +@liveexample{The following code shows how an `invalid_iterator` exception can be +caught.,invalid_iterator} + +@sa @ref exception for the base class of the library exceptions +@sa @ref parse_error for exceptions indicating a parse error +@sa @ref type_error for exceptions indicating executing a member function with + a wrong type +@sa @ref out_of_range for exceptions indicating access out of the defined range +@sa @ref other_error for exceptions indicating other library errors + +@since version 3.0.0 +*/ +class invalid_iterator : public exception +{ + public: + static invalid_iterator create(int id_, const std::string& what_arg) + { + std::string w = exception::name("invalid_iterator", id_) + what_arg; + return invalid_iterator(id_, w.c_str()); + } + + private: + invalid_iterator(int id_, const char* what_arg) + : exception(id_, what_arg) {} +}; + +/*! +@brief exception indicating executing a member function with a wrong type + +This exception is thrown in case of a type error; that is, a library function is +executed on a JSON value whose type does not match the expected semantics. + +Exceptions have ids 3xx. + +name / id | example message | description +----------------------------- | --------------- | ------------------------- +json.exception.type_error.301 | cannot create object from initializer list | To create an object from an initializer list, the initializer list must consist only of a list of pairs whose first element is a string. When this constraint is violated, an array is created instead. +json.exception.type_error.302 | type must be object, but is array | During implicit or explicit value conversion, the JSON type must be compatible to the target type. For instance, a JSON string can only be converted into string types, but not into numbers or boolean types. +json.exception.type_error.303 | incompatible ReferenceType for get_ref, actual type is object | To retrieve a reference to a value stored in a @ref basic_json object with @ref get_ref, the type of the reference must match the value type. For instance, for a JSON array, the @a ReferenceType must be @ref array_t&. +json.exception.type_error.304 | cannot use at() with string | The @ref at() member functions can only be executed for certain JSON types. +json.exception.type_error.305 | cannot use operator[] with string | The @ref operator[] member functions can only be executed for certain JSON types. +json.exception.type_error.306 | cannot use value() with string | The @ref value() member functions can only be executed for certain JSON types. +json.exception.type_error.307 | cannot use erase() with string | The @ref erase() member functions can only be executed for certain JSON types. +json.exception.type_error.308 | cannot use push_back() with string | The @ref push_back() and @ref operator+= member functions can only be executed for certain JSON types. +json.exception.type_error.309 | cannot use insert() with | The @ref insert() member functions can only be executed for certain JSON types. +json.exception.type_error.310 | cannot use swap() with number | The @ref swap() member functions can only be executed for certain JSON types. +json.exception.type_error.311 | cannot use emplace_back() with string | The @ref emplace_back() member function can only be executed for certain JSON types. +json.exception.type_error.312 | cannot use update() with string | The @ref update() member functions can only be executed for certain JSON types. +json.exception.type_error.313 | invalid value to unflatten | The @ref unflatten function converts an object whose keys are JSON Pointers back into an arbitrary nested JSON value. The JSON Pointers must not overlap, because then the resulting value would not be well defined. +json.exception.type_error.314 | only objects can be unflattened | The @ref unflatten function only works for an object whose keys are JSON Pointers. +json.exception.type_error.315 | values in object must be primitive | The @ref unflatten function only works for an object whose keys are JSON Pointers and whose values are primitive. +json.exception.type_error.316 | invalid UTF-8 byte at index 10: 0x7E | The @ref dump function only works with UTF-8 encoded strings; that is, if you assign a `std::string` to a JSON value, make sure it is UTF-8 encoded. | +json.exception.type_error.317 | JSON value cannot be serialized to requested format | The dynamic type of the object cannot be represented in the requested serialization format (e.g. a raw `true` or `null` JSON object cannot be serialized to BSON) | + +@liveexample{The following code shows how a `type_error` exception can be +caught.,type_error} + +@sa @ref exception for the base class of the library exceptions +@sa @ref parse_error for exceptions indicating a parse error +@sa @ref invalid_iterator for exceptions indicating errors with iterators +@sa @ref out_of_range for exceptions indicating access out of the defined range +@sa @ref other_error for exceptions indicating other library errors + +@since version 3.0.0 +*/ +class type_error : public exception +{ + public: + static type_error create(int id_, const std::string& what_arg) + { + std::string w = exception::name("type_error", id_) + what_arg; + return type_error(id_, w.c_str()); + } + + private: + type_error(int id_, const char* what_arg) : exception(id_, what_arg) {} +}; + +/*! +@brief exception indicating access out of the defined range + +This exception is thrown in case a library function is called on an input +parameter that exceeds the expected range, for instance in case of array +indices or nonexisting object keys. + +Exceptions have ids 4xx. + +name / id | example message | description +------------------------------- | --------------- | ------------------------- +json.exception.out_of_range.401 | array index 3 is out of range | The provided array index @a i is larger than @a size-1. +json.exception.out_of_range.402 | array index '-' (3) is out of range | The special array index `-` in a JSON Pointer never describes a valid element of the array, but the index past the end. That is, it can only be used to add elements at this position, but not to read it. +json.exception.out_of_range.403 | key 'foo' not found | The provided key was not found in the JSON object. +json.exception.out_of_range.404 | unresolved reference token 'foo' | A reference token in a JSON Pointer could not be resolved. +json.exception.out_of_range.405 | JSON pointer has no parent | The JSON Patch operations 'remove' and 'add' can not be applied to the root element of the JSON value. +json.exception.out_of_range.406 | number overflow parsing '10E1000' | A parsed number could not be stored as without changing it to NaN or INF. +json.exception.out_of_range.407 | number overflow serializing '9223372036854775808' | UBJSON and BSON only support integer numbers up to 9223372036854775807. | +json.exception.out_of_range.408 | excessive array size: 8658170730974374167 | The size (following `#`) of an UBJSON array or object exceeds the maximal capacity. | +json.exception.out_of_range.409 | BSON key cannot contain code point U+0000 (at byte 2) | Key identifiers to be serialized to BSON cannot contain code point U+0000, since the key is stored as zero-terminated c-string | + +@liveexample{The following code shows how an `out_of_range` exception can be +caught.,out_of_range} + +@sa @ref exception for the base class of the library exceptions +@sa @ref parse_error for exceptions indicating a parse error +@sa @ref invalid_iterator for exceptions indicating errors with iterators +@sa @ref type_error for exceptions indicating executing a member function with + a wrong type +@sa @ref other_error for exceptions indicating other library errors + +@since version 3.0.0 +*/ +class out_of_range : public exception +{ + public: + static out_of_range create(int id_, const std::string& what_arg) + { + std::string w = exception::name("out_of_range", id_) + what_arg; + return out_of_range(id_, w.c_str()); + } + + private: + out_of_range(int id_, const char* what_arg) : exception(id_, what_arg) {} +}; + +/*! +@brief exception indicating other library errors + +This exception is thrown in case of errors that cannot be classified with the +other exception types. + +Exceptions have ids 5xx. + +name / id | example message | description +------------------------------ | --------------- | ------------------------- +json.exception.other_error.501 | unsuccessful: {"op":"test","path":"/baz", "value":"bar"} | A JSON Patch operation 'test' failed. The unsuccessful operation is also printed. + +@sa @ref exception for the base class of the library exceptions +@sa @ref parse_error for exceptions indicating a parse error +@sa @ref invalid_iterator for exceptions indicating errors with iterators +@sa @ref type_error for exceptions indicating executing a member function with + a wrong type +@sa @ref out_of_range for exceptions indicating access out of the defined range + +@liveexample{The following code shows how an `other_error` exception can be +caught.,other_error} + +@since version 3.0.0 +*/ +class other_error : public exception +{ + public: + static other_error create(int id_, const std::string& what_arg) + { + std::string w = exception::name("other_error", id_) + what_arg; + return other_error(id_, w.c_str()); + } + + private: + other_error(int id_, const char* what_arg) : exception(id_, what_arg) {} +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/value_t.hpp> + + +#include <array> // array +#include <ciso646> // and +#include <cstddef> // size_t +#include <cstdint> // uint8_t + +namespace nlohmann +{ +namespace detail +{ +/////////////////////////// +// JSON type enumeration // +/////////////////////////// + +/*! +@brief the JSON type enumeration + +This enumeration collects the different JSON types. It is internally used to +distinguish the stored values, and the functions @ref basic_json::is_null(), +@ref basic_json::is_object(), @ref basic_json::is_array(), +@ref basic_json::is_string(), @ref basic_json::is_boolean(), +@ref basic_json::is_number() (with @ref basic_json::is_number_integer(), +@ref basic_json::is_number_unsigned(), and @ref basic_json::is_number_float()), +@ref basic_json::is_discarded(), @ref basic_json::is_primitive(), and +@ref basic_json::is_structured() rely on it. + +@note There are three enumeration entries (number_integer, number_unsigned, and +number_float), because the library distinguishes these three types for numbers: +@ref basic_json::number_unsigned_t is used for unsigned integers, +@ref basic_json::number_integer_t is used for signed integers, and +@ref basic_json::number_float_t is used for floating-point numbers or to +approximate integers which do not fit in the limits of their respective type. + +@sa @ref basic_json::basic_json(const value_t value_type) -- create a JSON +value with the default value for a given type + +@since version 1.0.0 +*/ +enum class value_t : std::uint8_t +{ + null, ///< null value + object, ///< object (unordered set of name/value pairs) + array, ///< array (ordered collection of values) + string, ///< string value + boolean, ///< boolean value + number_integer, ///< number value (signed integer) + number_unsigned, ///< number value (unsigned integer) + number_float, ///< number value (floating-point) + discarded ///< discarded by the the parser callback function +}; + +/*! +@brief comparison operator for JSON types + +Returns an ordering that is similar to Python: +- order: null < boolean < number < object < array < string +- furthermore, each type is not smaller than itself +- discarded values are not comparable + +@since version 1.0.0 +*/ +inline bool operator<(const value_t lhs, const value_t rhs) noexcept +{ + static constexpr std::array<std::uint8_t, 8> order = {{ + 0 /* null */, 3 /* object */, 4 /* array */, 5 /* string */, + 1 /* boolean */, 2 /* integer */, 2 /* unsigned */, 2 /* float */ + } + }; + + const auto l_index = static_cast<std::size_t>(lhs); + const auto r_index = static_cast<std::size_t>(rhs); + return l_index < order.size() and r_index < order.size() and order[l_index] < order[r_index]; +} +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/conversions/from_json.hpp> + + +#include <algorithm> // transform +#include <array> // array +#include <ciso646> // and, not +#include <forward_list> // forward_list +#include <iterator> // inserter, front_inserter, end +#include <map> // map +#include <string> // string +#include <tuple> // tuple, make_tuple +#include <type_traits> // is_arithmetic, is_same, is_enum, underlying_type, is_convertible +#include <unordered_map> // unordered_map +#include <utility> // pair, declval +#include <valarray> // valarray + +// #include <nlohmann/detail/exceptions.hpp> + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/meta/cpp_future.hpp> + +// #include <nlohmann/detail/meta/type_traits.hpp> + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +template<typename BasicJsonType> +void from_json(const BasicJsonType& j, typename std::nullptr_t& n) +{ + if (JSON_UNLIKELY(not j.is_null())) + { + JSON_THROW(type_error::create(302, "type must be null, but is " + std::string(j.type_name()))); + } + n = nullptr; +} + +// overloads for basic_json template parameters +template<typename BasicJsonType, typename ArithmeticType, + enable_if_t<std::is_arithmetic<ArithmeticType>::value and + not std::is_same<ArithmeticType, typename BasicJsonType::boolean_t>::value, + int> = 0> +void get_arithmetic_value(const BasicJsonType& j, ArithmeticType& val) +{ + switch (static_cast<value_t>(j)) + { + case value_t::number_unsigned: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_unsigned_t*>()); + break; + } + case value_t::number_integer: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_integer_t*>()); + break; + } + case value_t::number_float: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_float_t*>()); + break; + } + + default: + JSON_THROW(type_error::create(302, "type must be number, but is " + std::string(j.type_name()))); + } +} + +template<typename BasicJsonType> +void from_json(const BasicJsonType& j, typename BasicJsonType::boolean_t& b) +{ + if (JSON_UNLIKELY(not j.is_boolean())) + { + JSON_THROW(type_error::create(302, "type must be boolean, but is " + std::string(j.type_name()))); + } + b = *j.template get_ptr<const typename BasicJsonType::boolean_t*>(); +} + +template<typename BasicJsonType> +void from_json(const BasicJsonType& j, typename BasicJsonType::string_t& s) +{ + if (JSON_UNLIKELY(not j.is_string())) + { + JSON_THROW(type_error::create(302, "type must be string, but is " + std::string(j.type_name()))); + } + s = *j.template get_ptr<const typename BasicJsonType::string_t*>(); +} + +template < + typename BasicJsonType, typename ConstructibleStringType, + enable_if_t < + is_constructible_string_type<BasicJsonType, ConstructibleStringType>::value and + not std::is_same<typename BasicJsonType::string_t, + ConstructibleStringType>::value, + int > = 0 > +void from_json(const BasicJsonType& j, ConstructibleStringType& s) +{ + if (JSON_UNLIKELY(not j.is_string())) + { + JSON_THROW(type_error::create(302, "type must be string, but is " + std::string(j.type_name()))); + } + + s = *j.template get_ptr<const typename BasicJsonType::string_t*>(); +} + +template<typename BasicJsonType> +void from_json(const BasicJsonType& j, typename BasicJsonType::number_float_t& val) +{ + get_arithmetic_value(j, val); +} + +template<typename BasicJsonType> +void from_json(const BasicJsonType& j, typename BasicJsonType::number_unsigned_t& val) +{ + get_arithmetic_value(j, val); +} + +template<typename BasicJsonType> +void from_json(const BasicJsonType& j, typename BasicJsonType::number_integer_t& val) +{ + get_arithmetic_value(j, val); +} + +template<typename BasicJsonType, typename EnumType, + enable_if_t<std::is_enum<EnumType>::value, int> = 0> +void from_json(const BasicJsonType& j, EnumType& e) +{ + typename std::underlying_type<EnumType>::type val; + get_arithmetic_value(j, val); + e = static_cast<EnumType>(val); +} + +// forward_list doesn't have an insert method +template<typename BasicJsonType, typename T, typename Allocator, + enable_if_t<std::is_convertible<BasicJsonType, T>::value, int> = 0> +void from_json(const BasicJsonType& j, std::forward_list<T, Allocator>& l) +{ + if (JSON_UNLIKELY(not j.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + std::string(j.type_name()))); + } + std::transform(j.rbegin(), j.rend(), + std::front_inserter(l), [](const BasicJsonType & i) + { + return i.template get<T>(); + }); +} + +// valarray doesn't have an insert method +template<typename BasicJsonType, typename T, + enable_if_t<std::is_convertible<BasicJsonType, T>::value, int> = 0> +void from_json(const BasicJsonType& j, std::valarray<T>& l) +{ + if (JSON_UNLIKELY(not j.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + std::string(j.type_name()))); + } + l.resize(j.size()); + std::copy(j.m_value.array->begin(), j.m_value.array->end(), std::begin(l)); +} + +template<typename BasicJsonType> +void from_json_array_impl(const BasicJsonType& j, typename BasicJsonType::array_t& arr, priority_tag<3> /*unused*/) +{ + arr = *j.template get_ptr<const typename BasicJsonType::array_t*>(); +} + +template <typename BasicJsonType, typename T, std::size_t N> +auto from_json_array_impl(const BasicJsonType& j, std::array<T, N>& arr, + priority_tag<2> /*unused*/) +-> decltype(j.template get<T>(), void()) +{ + for (std::size_t i = 0; i < N; ++i) + { + arr[i] = j.at(i).template get<T>(); + } +} + +template<typename BasicJsonType, typename ConstructibleArrayType> +auto from_json_array_impl(const BasicJsonType& j, ConstructibleArrayType& arr, priority_tag<1> /*unused*/) +-> decltype( + arr.reserve(std::declval<typename ConstructibleArrayType::size_type>()), + j.template get<typename ConstructibleArrayType::value_type>(), + void()) +{ + using std::end; + + arr.reserve(j.size()); + std::transform(j.begin(), j.end(), + std::inserter(arr, end(arr)), [](const BasicJsonType & i) + { + // get<BasicJsonType>() returns *this, this won't call a from_json + // method when value_type is BasicJsonType + return i.template get<typename ConstructibleArrayType::value_type>(); + }); +} + +template <typename BasicJsonType, typename ConstructibleArrayType> +void from_json_array_impl(const BasicJsonType& j, ConstructibleArrayType& arr, + priority_tag<0> /*unused*/) +{ + using std::end; + + std::transform( + j.begin(), j.end(), std::inserter(arr, end(arr)), + [](const BasicJsonType & i) + { + // get<BasicJsonType>() returns *this, this won't call a from_json + // method when value_type is BasicJsonType + return i.template get<typename ConstructibleArrayType::value_type>(); + }); +} + +template <typename BasicJsonType, typename ConstructibleArrayType, + enable_if_t < + is_constructible_array_type<BasicJsonType, ConstructibleArrayType>::value and + not is_constructible_object_type<BasicJsonType, ConstructibleArrayType>::value and + not is_constructible_string_type<BasicJsonType, ConstructibleArrayType>::value and + not is_basic_json<ConstructibleArrayType>::value, + int > = 0 > + +auto from_json(const BasicJsonType& j, ConstructibleArrayType& arr) +-> decltype(from_json_array_impl(j, arr, priority_tag<3> {}), +j.template get<typename ConstructibleArrayType::value_type>(), +void()) +{ + if (JSON_UNLIKELY(not j.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + + std::string(j.type_name()))); + } + + from_json_array_impl(j, arr, priority_tag<3> {}); +} + +template<typename BasicJsonType, typename ConstructibleObjectType, + enable_if_t<is_constructible_object_type<BasicJsonType, ConstructibleObjectType>::value, int> = 0> +void from_json(const BasicJsonType& j, ConstructibleObjectType& obj) +{ + if (JSON_UNLIKELY(not j.is_object())) + { + JSON_THROW(type_error::create(302, "type must be object, but is " + std::string(j.type_name()))); + } + + auto inner_object = j.template get_ptr<const typename BasicJsonType::object_t*>(); + using value_type = typename ConstructibleObjectType::value_type; + std::transform( + inner_object->begin(), inner_object->end(), + std::inserter(obj, obj.begin()), + [](typename BasicJsonType::object_t::value_type const & p) + { + return value_type(p.first, p.second.template get<typename ConstructibleObjectType::mapped_type>()); + }); +} + +// overload for arithmetic types, not chosen for basic_json template arguments +// (BooleanType, etc..); note: Is it really necessary to provide explicit +// overloads for boolean_t etc. in case of a custom BooleanType which is not +// an arithmetic type? +template<typename BasicJsonType, typename ArithmeticType, + enable_if_t < + std::is_arithmetic<ArithmeticType>::value and + not std::is_same<ArithmeticType, typename BasicJsonType::number_unsigned_t>::value and + not std::is_same<ArithmeticType, typename BasicJsonType::number_integer_t>::value and + not std::is_same<ArithmeticType, typename BasicJsonType::number_float_t>::value and + not std::is_same<ArithmeticType, typename BasicJsonType::boolean_t>::value, + int> = 0> +void from_json(const BasicJsonType& j, ArithmeticType& val) +{ + switch (static_cast<value_t>(j)) + { + case value_t::number_unsigned: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_unsigned_t*>()); + break; + } + case value_t::number_integer: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_integer_t*>()); + break; + } + case value_t::number_float: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::number_float_t*>()); + break; + } + case value_t::boolean: + { + val = static_cast<ArithmeticType>(*j.template get_ptr<const typename BasicJsonType::boolean_t*>()); + break; + } + + default: + JSON_THROW(type_error::create(302, "type must be number, but is " + std::string(j.type_name()))); + } +} + +template<typename BasicJsonType, typename A1, typename A2> +void from_json(const BasicJsonType& j, std::pair<A1, A2>& p) +{ + p = {j.at(0).template get<A1>(), j.at(1).template get<A2>()}; +} + +template<typename BasicJsonType, typename Tuple, std::size_t... Idx> +void from_json_tuple_impl(const BasicJsonType& j, Tuple& t, index_sequence<Idx...> /*unused*/) +{ + t = std::make_tuple(j.at(Idx).template get<typename std::tuple_element<Idx, Tuple>::type>()...); +} + +template<typename BasicJsonType, typename... Args> +void from_json(const BasicJsonType& j, std::tuple<Args...>& t) +{ + from_json_tuple_impl(j, t, index_sequence_for<Args...> {}); +} + +template <typename BasicJsonType, typename Key, typename Value, typename Compare, typename Allocator, + typename = enable_if_t<not std::is_constructible< + typename BasicJsonType::string_t, Key>::value>> +void from_json(const BasicJsonType& j, std::map<Key, Value, Compare, Allocator>& m) +{ + if (JSON_UNLIKELY(not j.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + std::string(j.type_name()))); + } + for (const auto& p : j) + { + if (JSON_UNLIKELY(not p.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + std::string(p.type_name()))); + } + m.emplace(p.at(0).template get<Key>(), p.at(1).template get<Value>()); + } +} + +template <typename BasicJsonType, typename Key, typename Value, typename Hash, typename KeyEqual, typename Allocator, + typename = enable_if_t<not std::is_constructible< + typename BasicJsonType::string_t, Key>::value>> +void from_json(const BasicJsonType& j, std::unordered_map<Key, Value, Hash, KeyEqual, Allocator>& m) +{ + if (JSON_UNLIKELY(not j.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + std::string(j.type_name()))); + } + for (const auto& p : j) + { + if (JSON_UNLIKELY(not p.is_array())) + { + JSON_THROW(type_error::create(302, "type must be array, but is " + std::string(p.type_name()))); + } + m.emplace(p.at(0).template get<Key>(), p.at(1).template get<Value>()); + } +} + +struct from_json_fn +{ + template<typename BasicJsonType, typename T> + auto operator()(const BasicJsonType& j, T& val) const + noexcept(noexcept(from_json(j, val))) + -> decltype(from_json(j, val), void()) + { + return from_json(j, val); + } +}; +} // namespace detail + +/// namespace to hold default `from_json` function +/// to see why this is required: +/// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4381.html +namespace +{ +constexpr const auto& from_json = detail::static_const<detail::from_json_fn>::value; +} // namespace +} // namespace nlohmann + +// #include <nlohmann/detail/conversions/to_json.hpp> + + +#include <ciso646> // or, and, not +#include <iterator> // begin, end +#include <tuple> // tuple, get +#include <type_traits> // is_same, is_constructible, is_floating_point, is_enum, underlying_type +#include <utility> // move, forward, declval, pair +#include <valarray> // valarray +#include <vector> // vector + +// #include <nlohmann/detail/meta/cpp_future.hpp> + +// #include <nlohmann/detail/meta/type_traits.hpp> + +// #include <nlohmann/detail/value_t.hpp> + +// #include <nlohmann/detail/iterators/iteration_proxy.hpp> + + +#include <cstddef> // size_t +#include <string> // string, to_string +#include <iterator> // input_iterator_tag + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/// proxy class for the items() function +template<typename IteratorType> class iteration_proxy +{ + private: + /// helper class for iteration + class iteration_proxy_internal + { + public: + using difference_type = std::ptrdiff_t; + using value_type = iteration_proxy_internal; + using pointer = iteration_proxy_internal*; + using reference = iteration_proxy_internal&; + using iterator_category = std::input_iterator_tag; + + private: + /// the iterator + IteratorType anchor; + /// an index for arrays (used to create key names) + std::size_t array_index = 0; + /// last stringified array index + mutable std::size_t array_index_last = 0; + /// a string representation of the array index + mutable std::string array_index_str = "0"; + /// an empty string (to return a reference for primitive values) + const std::string empty_str = ""; + + public: + explicit iteration_proxy_internal(IteratorType it) noexcept : anchor(it) {} + + /// dereference operator (needed for range-based for) + iteration_proxy_internal& operator*() + { + return *this; + } + + /// increment operator (needed for range-based for) + iteration_proxy_internal& operator++() + { + ++anchor; + ++array_index; + + return *this; + } + + /// equality operator (needed for InputIterator) + bool operator==(const iteration_proxy_internal& o) const noexcept + { + return anchor == o.anchor; + } + + /// inequality operator (needed for range-based for) + bool operator!=(const iteration_proxy_internal& o) const noexcept + { + return anchor != o.anchor; + } + + /// return key of the iterator + const std::string& key() const + { + assert(anchor.m_object != nullptr); + + switch (anchor.m_object->type()) + { + // use integer array index as key + case value_t::array: + { + if (array_index != array_index_last) + { + array_index_str = std::to_string(array_index); + array_index_last = array_index; + } + return array_index_str; + } + + // use key from the object + case value_t::object: + return anchor.key(); + + // use an empty key for all primitive types + default: + return empty_str; + } + } + + /// return value of the iterator + typename IteratorType::reference value() const + { + return anchor.value(); + } + }; + + /// the container to iterate + typename IteratorType::reference container; + + public: + /// construct iteration proxy from a container + explicit iteration_proxy(typename IteratorType::reference cont) noexcept + : container(cont) {} + + /// return iterator begin (needed for range-based for) + iteration_proxy_internal begin() noexcept + { + return iteration_proxy_internal(container.begin()); + } + + /// return iterator end (needed for range-based for) + iteration_proxy_internal end() noexcept + { + return iteration_proxy_internal(container.end()); + } +}; +} // namespace detail +} // namespace nlohmann + + +namespace nlohmann +{ +namespace detail +{ +////////////////// +// constructors // +////////////////// + +template<value_t> struct external_constructor; + +template<> +struct external_constructor<value_t::boolean> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::boolean_t b) noexcept + { + j.m_type = value_t::boolean; + j.m_value = b; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor<value_t::string> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, const typename BasicJsonType::string_t& s) + { + j.m_type = value_t::string; + j.m_value = s; + j.assert_invariant(); + } + + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::string_t&& s) + { + j.m_type = value_t::string; + j.m_value = std::move(s); + j.assert_invariant(); + } + + template<typename BasicJsonType, typename CompatibleStringType, + enable_if_t<not std::is_same<CompatibleStringType, typename BasicJsonType::string_t>::value, + int> = 0> + static void construct(BasicJsonType& j, const CompatibleStringType& str) + { + j.m_type = value_t::string; + j.m_value.string = j.template create<typename BasicJsonType::string_t>(str); + j.assert_invariant(); + } +}; + +template<> +struct external_constructor<value_t::number_float> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::number_float_t val) noexcept + { + j.m_type = value_t::number_float; + j.m_value = val; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor<value_t::number_unsigned> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::number_unsigned_t val) noexcept + { + j.m_type = value_t::number_unsigned; + j.m_value = val; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor<value_t::number_integer> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::number_integer_t val) noexcept + { + j.m_type = value_t::number_integer; + j.m_value = val; + j.assert_invariant(); + } +}; + +template<> +struct external_constructor<value_t::array> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, const typename BasicJsonType::array_t& arr) + { + j.m_type = value_t::array; + j.m_value = arr; + j.assert_invariant(); + } + + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::array_t&& arr) + { + j.m_type = value_t::array; + j.m_value = std::move(arr); + j.assert_invariant(); + } + + template<typename BasicJsonType, typename CompatibleArrayType, + enable_if_t<not std::is_same<CompatibleArrayType, typename BasicJsonType::array_t>::value, + int> = 0> + static void construct(BasicJsonType& j, const CompatibleArrayType& arr) + { + using std::begin; + using std::end; + j.m_type = value_t::array; + j.m_value.array = j.template create<typename BasicJsonType::array_t>(begin(arr), end(arr)); + j.assert_invariant(); + } + + template<typename BasicJsonType> + static void construct(BasicJsonType& j, const std::vector<bool>& arr) + { + j.m_type = value_t::array; + j.m_value = value_t::array; + j.m_value.array->reserve(arr.size()); + for (const bool x : arr) + { + j.m_value.array->push_back(x); + } + j.assert_invariant(); + } + + template<typename BasicJsonType, typename T, + enable_if_t<std::is_convertible<T, BasicJsonType>::value, int> = 0> + static void construct(BasicJsonType& j, const std::valarray<T>& arr) + { + j.m_type = value_t::array; + j.m_value = value_t::array; + j.m_value.array->resize(arr.size()); + std::copy(std::begin(arr), std::end(arr), j.m_value.array->begin()); + j.assert_invariant(); + } +}; + +template<> +struct external_constructor<value_t::object> +{ + template<typename BasicJsonType> + static void construct(BasicJsonType& j, const typename BasicJsonType::object_t& obj) + { + j.m_type = value_t::object; + j.m_value = obj; + j.assert_invariant(); + } + + template<typename BasicJsonType> + static void construct(BasicJsonType& j, typename BasicJsonType::object_t&& obj) + { + j.m_type = value_t::object; + j.m_value = std::move(obj); + j.assert_invariant(); + } + + template<typename BasicJsonType, typename CompatibleObjectType, + enable_if_t<not std::is_same<CompatibleObjectType, typename BasicJsonType::object_t>::value, int> = 0> + static void construct(BasicJsonType& j, const CompatibleObjectType& obj) + { + using std::begin; + using std::end; + + j.m_type = value_t::object; + j.m_value.object = j.template create<typename BasicJsonType::object_t>(begin(obj), end(obj)); + j.assert_invariant(); + } +}; + +///////////// +// to_json // +///////////// + +template<typename BasicJsonType, typename T, + enable_if_t<std::is_same<T, typename BasicJsonType::boolean_t>::value, int> = 0> +void to_json(BasicJsonType& j, T b) noexcept +{ + external_constructor<value_t::boolean>::construct(j, b); +} + +template<typename BasicJsonType, typename CompatibleString, + enable_if_t<std::is_constructible<typename BasicJsonType::string_t, CompatibleString>::value, int> = 0> +void to_json(BasicJsonType& j, const CompatibleString& s) +{ + external_constructor<value_t::string>::construct(j, s); +} + +template<typename BasicJsonType> +void to_json(BasicJsonType& j, typename BasicJsonType::string_t&& s) +{ + external_constructor<value_t::string>::construct(j, std::move(s)); +} + +template<typename BasicJsonType, typename FloatType, + enable_if_t<std::is_floating_point<FloatType>::value, int> = 0> +void to_json(BasicJsonType& j, FloatType val) noexcept +{ + external_constructor<value_t::number_float>::construct(j, static_cast<typename BasicJsonType::number_float_t>(val)); +} + +template<typename BasicJsonType, typename CompatibleNumberUnsignedType, + enable_if_t<is_compatible_integer_type<typename BasicJsonType::number_unsigned_t, CompatibleNumberUnsignedType>::value, int> = 0> +void to_json(BasicJsonType& j, CompatibleNumberUnsignedType val) noexcept +{ + external_constructor<value_t::number_unsigned>::construct(j, static_cast<typename BasicJsonType::number_unsigned_t>(val)); +} + +template<typename BasicJsonType, typename CompatibleNumberIntegerType, + enable_if_t<is_compatible_integer_type<typename BasicJsonType::number_integer_t, CompatibleNumberIntegerType>::value, int> = 0> +void to_json(BasicJsonType& j, CompatibleNumberIntegerType val) noexcept +{ + external_constructor<value_t::number_integer>::construct(j, static_cast<typename BasicJsonType::number_integer_t>(val)); +} + +template<typename BasicJsonType, typename EnumType, + enable_if_t<std::is_enum<EnumType>::value, int> = 0> +void to_json(BasicJsonType& j, EnumType e) noexcept +{ + using underlying_type = typename std::underlying_type<EnumType>::type; + external_constructor<value_t::number_integer>::construct(j, static_cast<underlying_type>(e)); +} + +template<typename BasicJsonType> +void to_json(BasicJsonType& j, const std::vector<bool>& e) +{ + external_constructor<value_t::array>::construct(j, e); +} + +template <typename BasicJsonType, typename CompatibleArrayType, + enable_if_t<is_compatible_array_type<BasicJsonType, + CompatibleArrayType>::value and + not is_compatible_object_type< + BasicJsonType, CompatibleArrayType>::value and + not is_compatible_string_type<BasicJsonType, CompatibleArrayType>::value and + not is_basic_json<CompatibleArrayType>::value, + int> = 0> +void to_json(BasicJsonType& j, const CompatibleArrayType& arr) +{ + external_constructor<value_t::array>::construct(j, arr); +} + +template<typename BasicJsonType, typename T, + enable_if_t<std::is_convertible<T, BasicJsonType>::value, int> = 0> +void to_json(BasicJsonType& j, const std::valarray<T>& arr) +{ + external_constructor<value_t::array>::construct(j, std::move(arr)); +} + +template<typename BasicJsonType> +void to_json(BasicJsonType& j, typename BasicJsonType::array_t&& arr) +{ + external_constructor<value_t::array>::construct(j, std::move(arr)); +} + +template<typename BasicJsonType, typename CompatibleObjectType, + enable_if_t<is_compatible_object_type<BasicJsonType, CompatibleObjectType>::value and not is_basic_json<CompatibleObjectType>::value, int> = 0> +void to_json(BasicJsonType& j, const CompatibleObjectType& obj) +{ + external_constructor<value_t::object>::construct(j, obj); +} + +template<typename BasicJsonType> +void to_json(BasicJsonType& j, typename BasicJsonType::object_t&& obj) +{ + external_constructor<value_t::object>::construct(j, std::move(obj)); +} + +template < + typename BasicJsonType, typename T, std::size_t N, + enable_if_t<not std::is_constructible<typename BasicJsonType::string_t, + const T (&)[N]>::value, + int> = 0 > +void to_json(BasicJsonType& j, const T (&arr)[N]) +{ + external_constructor<value_t::array>::construct(j, arr); +} + +template<typename BasicJsonType, typename... Args> +void to_json(BasicJsonType& j, const std::pair<Args...>& p) +{ + j = {p.first, p.second}; +} + +// for https://github.com/nlohmann/json/pull/1134 +template<typename BasicJsonType, typename T, + enable_if_t<std::is_same<T, typename iteration_proxy<typename BasicJsonType::iterator>::iteration_proxy_internal>::value, int> = 0> +void to_json(BasicJsonType& j, const T& b) +{ + j = {{b.key(), b.value()}}; +} + +template<typename BasicJsonType, typename Tuple, std::size_t... Idx> +void to_json_tuple_impl(BasicJsonType& j, const Tuple& t, index_sequence<Idx...> /*unused*/) +{ + j = {std::get<Idx>(t)...}; +} + +template<typename BasicJsonType, typename... Args> +void to_json(BasicJsonType& j, const std::tuple<Args...>& t) +{ + to_json_tuple_impl(j, t, index_sequence_for<Args...> {}); +} + +struct to_json_fn +{ + template<typename BasicJsonType, typename T> + auto operator()(BasicJsonType& j, T&& val) const noexcept(noexcept(to_json(j, std::forward<T>(val)))) + -> decltype(to_json(j, std::forward<T>(val)), void()) + { + return to_json(j, std::forward<T>(val)); + } +}; +} // namespace detail + +/// namespace to hold default `to_json` function +namespace +{ +constexpr const auto& to_json = detail::static_const<detail::to_json_fn>::value; +} // namespace +} // namespace nlohmann + +// #include <nlohmann/detail/input/input_adapters.hpp> + + +#include <cassert> // assert +#include <cstddef> // size_t +#include <cstring> // strlen +#include <istream> // istream +#include <iterator> // begin, end, iterator_traits, random_access_iterator_tag, distance, next +#include <memory> // shared_ptr, make_shared, addressof +#include <numeric> // accumulate +#include <string> // string, char_traits +#include <type_traits> // enable_if, is_base_of, is_pointer, is_integral, remove_pointer +#include <utility> // pair, declval + +// #include <nlohmann/detail/macro_scope.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/// the supported input formats +enum class input_format_t { json, cbor, msgpack, ubjson, bson }; + +//////////////////// +// input adapters // +//////////////////// + +/*! +@brief abstract input adapter interface + +Produces a stream of std::char_traits<char>::int_type characters from a +std::istream, a buffer, or some other input type. Accepts the return of +exactly one non-EOF character for future input. The int_type characters +returned consist of all valid char values as positive values (typically +unsigned char), plus an EOF value outside that range, specified by the value +of the function std::char_traits<char>::eof(). This value is typically -1, but +could be any arbitrary value which is not a valid char value. +*/ +struct input_adapter_protocol +{ + /// get a character [0,255] or std::char_traits<char>::eof(). + virtual std::char_traits<char>::int_type get_character() = 0; + virtual ~input_adapter_protocol() = default; +}; + +/// a type to simplify interfaces +using input_adapter_t = std::shared_ptr<input_adapter_protocol>; + +/*! +Input adapter for a (caching) istream. Ignores a UFT Byte Order Mark at +beginning of input. Does not support changing the underlying std::streambuf +in mid-input. Maintains underlying std::istream and std::streambuf to support +subsequent use of standard std::istream operations to process any input +characters following those used in parsing the JSON input. Clears the +std::istream flags; any input errors (e.g., EOF) will be detected by the first +subsequent call for input from the std::istream. +*/ +class input_stream_adapter : public input_adapter_protocol +{ + public: + ~input_stream_adapter() override + { + // clear stream flags; we use underlying streambuf I/O, do not + // maintain ifstream flags + is.clear(); + } + + explicit input_stream_adapter(std::istream& i) + : is(i), sb(*i.rdbuf()) + {} + + // delete because of pointer members + input_stream_adapter(const input_stream_adapter&) = delete; + input_stream_adapter& operator=(input_stream_adapter&) = delete; + input_stream_adapter(input_stream_adapter&&) = delete; + input_stream_adapter& operator=(input_stream_adapter&&) = delete; + + // std::istream/std::streambuf use std::char_traits<char>::to_int_type, to + // ensure that std::char_traits<char>::eof() and the character 0xFF do not + // end up as the same value, eg. 0xFFFFFFFF. + std::char_traits<char>::int_type get_character() override + { + return sb.sbumpc(); + } + + private: + /// the associated input stream + std::istream& is; + std::streambuf& sb; +}; + +/// input adapter for buffer input +class input_buffer_adapter : public input_adapter_protocol +{ + public: + input_buffer_adapter(const char* b, const std::size_t l) noexcept + : cursor(b), limit(b + l) + {} + + // delete because of pointer members + input_buffer_adapter(const input_buffer_adapter&) = delete; + input_buffer_adapter& operator=(input_buffer_adapter&) = delete; + input_buffer_adapter(input_buffer_adapter&&) = delete; + input_buffer_adapter& operator=(input_buffer_adapter&&) = delete; + ~input_buffer_adapter() override = default; + + std::char_traits<char>::int_type get_character() noexcept override + { + if (JSON_LIKELY(cursor < limit)) + { + return std::char_traits<char>::to_int_type(*(cursor++)); + } + + return std::char_traits<char>::eof(); + } + + private: + /// pointer to the current character + const char* cursor; + /// pointer past the last character + const char* const limit; +}; + +template<typename WideStringType, size_t T> +struct wide_string_input_helper +{ + // UTF-32 + static void fill_buffer(const WideStringType& str, size_t& current_wchar, std::array<std::char_traits<char>::int_type, 4>& utf8_bytes, size_t& utf8_bytes_index, size_t& utf8_bytes_filled) + { + utf8_bytes_index = 0; + + if (current_wchar == str.size()) + { + utf8_bytes[0] = std::char_traits<char>::eof(); + utf8_bytes_filled = 1; + } + else + { + // get the current character + const auto wc = static_cast<int>(str[current_wchar++]); + + // UTF-32 to UTF-8 encoding + if (wc < 0x80) + { + utf8_bytes[0] = wc; + utf8_bytes_filled = 1; + } + else if (wc <= 0x7FF) + { + utf8_bytes[0] = 0xC0 | ((wc >> 6) & 0x1F); + utf8_bytes[1] = 0x80 | (wc & 0x3F); + utf8_bytes_filled = 2; + } + else if (wc <= 0xFFFF) + { + utf8_bytes[0] = 0xE0 | ((wc >> 12) & 0x0F); + utf8_bytes[1] = 0x80 | ((wc >> 6) & 0x3F); + utf8_bytes[2] = 0x80 | (wc & 0x3F); + utf8_bytes_filled = 3; + } + else if (wc <= 0x10FFFF) + { + utf8_bytes[0] = 0xF0 | ((wc >> 18) & 0x07); + utf8_bytes[1] = 0x80 | ((wc >> 12) & 0x3F); + utf8_bytes[2] = 0x80 | ((wc >> 6) & 0x3F); + utf8_bytes[3] = 0x80 | (wc & 0x3F); + utf8_bytes_filled = 4; + } + else + { + // unknown character + utf8_bytes[0] = wc; + utf8_bytes_filled = 1; + } + } + } +}; + +template<typename WideStringType> +struct wide_string_input_helper<WideStringType, 2> +{ + // UTF-16 + static void fill_buffer(const WideStringType& str, size_t& current_wchar, std::array<std::char_traits<char>::int_type, 4>& utf8_bytes, size_t& utf8_bytes_index, size_t& utf8_bytes_filled) + { + utf8_bytes_index = 0; + + if (current_wchar == str.size()) + { + utf8_bytes[0] = std::char_traits<char>::eof(); + utf8_bytes_filled = 1; + } + else + { + // get the current character + const auto wc = static_cast<int>(str[current_wchar++]); + + // UTF-16 to UTF-8 encoding + if (wc < 0x80) + { + utf8_bytes[0] = wc; + utf8_bytes_filled = 1; + } + else if (wc <= 0x7FF) + { + utf8_bytes[0] = 0xC0 | ((wc >> 6)); + utf8_bytes[1] = 0x80 | (wc & 0x3F); + utf8_bytes_filled = 2; + } + else if (0xD800 > wc or wc >= 0xE000) + { + utf8_bytes[0] = 0xE0 | ((wc >> 12)); + utf8_bytes[1] = 0x80 | ((wc >> 6) & 0x3F); + utf8_bytes[2] = 0x80 | (wc & 0x3F); + utf8_bytes_filled = 3; + } + else + { + if (current_wchar < str.size()) + { + const auto wc2 = static_cast<int>(str[current_wchar++]); + const int charcode = 0x10000 + (((wc & 0x3FF) << 10) | (wc2 & 0x3FF)); + utf8_bytes[0] = 0xf0 | (charcode >> 18); + utf8_bytes[1] = 0x80 | ((charcode >> 12) & 0x3F); + utf8_bytes[2] = 0x80 | ((charcode >> 6) & 0x3F); + utf8_bytes[3] = 0x80 | (charcode & 0x3F); + utf8_bytes_filled = 4; + } + else + { + // unknown character + ++current_wchar; + utf8_bytes[0] = wc; + utf8_bytes_filled = 1; + } + } + } + } +}; + +template<typename WideStringType> +class wide_string_input_adapter : public input_adapter_protocol +{ + public: + explicit wide_string_input_adapter(const WideStringType& w) noexcept + : str(w) + {} + + std::char_traits<char>::int_type get_character() noexcept override + { + // check if buffer needs to be filled + if (utf8_bytes_index == utf8_bytes_filled) + { + fill_buffer<sizeof(typename WideStringType::value_type)>(); + + assert(utf8_bytes_filled > 0); + assert(utf8_bytes_index == 0); + } + + // use buffer + assert(utf8_bytes_filled > 0); + assert(utf8_bytes_index < utf8_bytes_filled); + return utf8_bytes[utf8_bytes_index++]; + } + + private: + template<size_t T> + void fill_buffer() + { + wide_string_input_helper<WideStringType, T>::fill_buffer(str, current_wchar, utf8_bytes, utf8_bytes_index, utf8_bytes_filled); + } + + /// the wstring to process + const WideStringType& str; + + /// index of the current wchar in str + std::size_t current_wchar = 0; + + /// a buffer for UTF-8 bytes + std::array<std::char_traits<char>::int_type, 4> utf8_bytes = {{0, 0, 0, 0}}; + + /// index to the utf8_codes array for the next valid byte + std::size_t utf8_bytes_index = 0; + /// number of valid bytes in the utf8_codes array + std::size_t utf8_bytes_filled = 0; +}; + +class input_adapter +{ + public: + // native support + + /// input adapter for input stream + input_adapter(std::istream& i) + : ia(std::make_shared<input_stream_adapter>(i)) {} + + /// input adapter for input stream + input_adapter(std::istream&& i) + : ia(std::make_shared<input_stream_adapter>(i)) {} + + input_adapter(const std::wstring& ws) + : ia(std::make_shared<wide_string_input_adapter<std::wstring>>(ws)) {} + + input_adapter(const std::u16string& ws) + : ia(std::make_shared<wide_string_input_adapter<std::u16string>>(ws)) {} + + input_adapter(const std::u32string& ws) + : ia(std::make_shared<wide_string_input_adapter<std::u32string>>(ws)) {} + + /// input adapter for buffer + template<typename CharT, + typename std::enable_if< + std::is_pointer<CharT>::value and + std::is_integral<typename std::remove_pointer<CharT>::type>::value and + sizeof(typename std::remove_pointer<CharT>::type) == 1, + int>::type = 0> + input_adapter(CharT b, std::size_t l) + : ia(std::make_shared<input_buffer_adapter>(reinterpret_cast<const char*>(b), l)) {} + + // derived support + + /// input adapter for string literal + template<typename CharT, + typename std::enable_if< + std::is_pointer<CharT>::value and + std::is_integral<typename std::remove_pointer<CharT>::type>::value and + sizeof(typename std::remove_pointer<CharT>::type) == 1, + int>::type = 0> + input_adapter(CharT b) + : input_adapter(reinterpret_cast<const char*>(b), + std::strlen(reinterpret_cast<const char*>(b))) {} + + /// input adapter for iterator range with contiguous storage + template<class IteratorType, + typename std::enable_if< + std::is_same<typename std::iterator_traits<IteratorType>::iterator_category, std::random_access_iterator_tag>::value, + int>::type = 0> + input_adapter(IteratorType first, IteratorType last) + { +#ifndef NDEBUG + // assertion to check that the iterator range is indeed contiguous, + // see http://stackoverflow.com/a/35008842/266378 for more discussion + const auto is_contiguous = std::accumulate( + first, last, std::pair<bool, int>(true, 0), + [&first](std::pair<bool, int> res, decltype(*first) val) + { + res.first &= (val == *(std::next(std::addressof(*first), res.second++))); + return res; + }).first; + assert(is_contiguous); +#endif + + // assertion to check that each element is 1 byte long + static_assert( + sizeof(typename std::iterator_traits<IteratorType>::value_type) == 1, + "each element in the iterator range must have the size of 1 byte"); + + const auto len = static_cast<size_t>(std::distance(first, last)); + if (JSON_LIKELY(len > 0)) + { + // there is at least one element: use the address of first + ia = std::make_shared<input_buffer_adapter>(reinterpret_cast<const char*>(&(*first)), len); + } + else + { + // the address of first cannot be used: use nullptr + ia = std::make_shared<input_buffer_adapter>(nullptr, len); + } + } + + /// input adapter for array + template<class T, std::size_t N> + input_adapter(T (&array)[N]) + : input_adapter(std::begin(array), std::end(array)) {} + + /// input adapter for contiguous container + template<class ContiguousContainer, typename + std::enable_if<not std::is_pointer<ContiguousContainer>::value and + std::is_base_of<std::random_access_iterator_tag, typename std::iterator_traits<decltype(std::begin(std::declval<ContiguousContainer const>()))>::iterator_category>::value, + int>::type = 0> + input_adapter(const ContiguousContainer& c) + : input_adapter(std::begin(c), std::end(c)) {} + + operator input_adapter_t() + { + return ia; + } + + private: + /// the actual adapter + input_adapter_t ia = nullptr; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/input/lexer.hpp> + + +#include <clocale> // localeconv +#include <cstddef> // size_t +#include <cstdlib> // strtof, strtod, strtold, strtoll, strtoull +#include <cstdio> // snprintf +#include <initializer_list> // initializer_list +#include <string> // char_traits, string +#include <vector> // vector + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/input/input_adapters.hpp> + +// #include <nlohmann/detail/input/position_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/////////// +// lexer // +/////////// + +/*! +@brief lexical analysis + +This class organizes the lexical analysis during JSON deserialization. +*/ +template<typename BasicJsonType> +class lexer +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + + public: + /// token types for the parser + enum class token_type + { + uninitialized, ///< indicating the scanner is uninitialized + literal_true, ///< the `true` literal + literal_false, ///< the `false` literal + literal_null, ///< the `null` literal + value_string, ///< a string -- use get_string() for actual value + value_unsigned, ///< an unsigned integer -- use get_number_unsigned() for actual value + value_integer, ///< a signed integer -- use get_number_integer() for actual value + value_float, ///< an floating point number -- use get_number_float() for actual value + begin_array, ///< the character for array begin `[` + begin_object, ///< the character for object begin `{` + end_array, ///< the character for array end `]` + end_object, ///< the character for object end `}` + name_separator, ///< the name separator `:` + value_separator, ///< the value separator `,` + parse_error, ///< indicating a parse error + end_of_input, ///< indicating the end of the input buffer + literal_or_value ///< a literal or the begin of a value (only for diagnostics) + }; + + /// return name of values of type token_type (only used for errors) + static const char* token_type_name(const token_type t) noexcept + { + switch (t) + { + case token_type::uninitialized: + return "<uninitialized>"; + case token_type::literal_true: + return "true literal"; + case token_type::literal_false: + return "false literal"; + case token_type::literal_null: + return "null literal"; + case token_type::value_string: + return "string literal"; + case lexer::token_type::value_unsigned: + case lexer::token_type::value_integer: + case lexer::token_type::value_float: + return "number literal"; + case token_type::begin_array: + return "'['"; + case token_type::begin_object: + return "'{'"; + case token_type::end_array: + return "']'"; + case token_type::end_object: + return "'}'"; + case token_type::name_separator: + return "':'"; + case token_type::value_separator: + return "','"; + case token_type::parse_error: + return "<parse error>"; + case token_type::end_of_input: + return "end of input"; + case token_type::literal_or_value: + return "'[', '{', or a literal"; + // LCOV_EXCL_START + default: // catch non-enum values + return "unknown token"; + // LCOV_EXCL_STOP + } + } + + explicit lexer(detail::input_adapter_t&& adapter) + : ia(std::move(adapter)), decimal_point_char(get_decimal_point()) {} + + // delete because of pointer members + lexer(const lexer&) = delete; + lexer(lexer&&) = delete; + lexer& operator=(lexer&) = delete; + lexer& operator=(lexer&&) = delete; + ~lexer() = default; + + private: + ///////////////////// + // locales + ///////////////////// + + /// return the locale-dependent decimal point + static char get_decimal_point() noexcept + { + const auto loc = localeconv(); + assert(loc != nullptr); + return (loc->decimal_point == nullptr) ? '.' : *(loc->decimal_point); + } + + ///////////////////// + // scan functions + ///////////////////// + + /*! + @brief get codepoint from 4 hex characters following `\u` + + For input "\u c1 c2 c3 c4" the codepoint is: + (c1 * 0x1000) + (c2 * 0x0100) + (c3 * 0x0010) + c4 + = (c1 << 12) + (c2 << 8) + (c3 << 4) + (c4 << 0) + + Furthermore, the possible characters '0'..'9', 'A'..'F', and 'a'..'f' + must be converted to the integers 0x0..0x9, 0xA..0xF, 0xA..0xF, resp. The + conversion is done by subtracting the offset (0x30, 0x37, and 0x57) + between the ASCII value of the character and the desired integer value. + + @return codepoint (0x0000..0xFFFF) or -1 in case of an error (e.g. EOF or + non-hex character) + */ + int get_codepoint() + { + // this function only makes sense after reading `\u` + assert(current == 'u'); + int codepoint = 0; + + const auto factors = { 12, 8, 4, 0 }; + for (const auto factor : factors) + { + get(); + + if (current >= '0' and current <= '9') + { + codepoint += ((current - 0x30) << factor); + } + else if (current >= 'A' and current <= 'F') + { + codepoint += ((current - 0x37) << factor); + } + else if (current >= 'a' and current <= 'f') + { + codepoint += ((current - 0x57) << factor); + } + else + { + return -1; + } + } + + assert(0x0000 <= codepoint and codepoint <= 0xFFFF); + return codepoint; + } + + /*! + @brief check if the next byte(s) are inside a given range + + Adds the current byte and, for each passed range, reads a new byte and + checks if it is inside the range. If a violation was detected, set up an + error message and return false. Otherwise, return true. + + @param[in] ranges list of integers; interpreted as list of pairs of + inclusive lower and upper bound, respectively + + @pre The passed list @a ranges must have 2, 4, or 6 elements; that is, + 1, 2, or 3 pairs. This precondition is enforced by an assertion. + + @return true if and only if no range violation was detected + */ + bool next_byte_in_range(std::initializer_list<int> ranges) + { + assert(ranges.size() == 2 or ranges.size() == 4 or ranges.size() == 6); + add(current); + + for (auto range = ranges.begin(); range != ranges.end(); ++range) + { + get(); + if (JSON_LIKELY(*range <= current and current <= *(++range))) + { + add(current); + } + else + { + error_message = "invalid string: ill-formed UTF-8 byte"; + return false; + } + } + + return true; + } + + /*! + @brief scan a string literal + + This function scans a string according to Sect. 7 of RFC 7159. While + scanning, bytes are escaped and copied into buffer token_buffer. Then the + function returns successfully, token_buffer is *not* null-terminated (as it + may contain \0 bytes), and token_buffer.size() is the number of bytes in the + string. + + @return token_type::value_string if string could be successfully scanned, + token_type::parse_error otherwise + + @note In case of errors, variable error_message contains a textual + description. + */ + token_type scan_string() + { + // reset token_buffer (ignore opening quote) + reset(); + + // we entered the function by reading an open quote + assert(current == '\"'); + + while (true) + { + // get next character + switch (get()) + { + // end of file while parsing string + case std::char_traits<char>::eof(): + { + error_message = "invalid string: missing closing quote"; + return token_type::parse_error; + } + + // closing quote + case '\"': + { + return token_type::value_string; + } + + // escapes + case '\\': + { + switch (get()) + { + // quotation mark + case '\"': + add('\"'); + break; + // reverse solidus + case '\\': + add('\\'); + break; + // solidus + case '/': + add('/'); + break; + // backspace + case 'b': + add('\b'); + break; + // form feed + case 'f': + add('\f'); + break; + // line feed + case 'n': + add('\n'); + break; + // carriage return + case 'r': + add('\r'); + break; + // tab + case 't': + add('\t'); + break; + + // unicode escapes + case 'u': + { + const int codepoint1 = get_codepoint(); + int codepoint = codepoint1; // start with codepoint1 + + if (JSON_UNLIKELY(codepoint1 == -1)) + { + error_message = "invalid string: '\\u' must be followed by 4 hex digits"; + return token_type::parse_error; + } + + // check if code point is a high surrogate + if (0xD800 <= codepoint1 and codepoint1 <= 0xDBFF) + { + // expect next \uxxxx entry + if (JSON_LIKELY(get() == '\\' and get() == 'u')) + { + const int codepoint2 = get_codepoint(); + + if (JSON_UNLIKELY(codepoint2 == -1)) + { + error_message = "invalid string: '\\u' must be followed by 4 hex digits"; + return token_type::parse_error; + } + + // check if codepoint2 is a low surrogate + if (JSON_LIKELY(0xDC00 <= codepoint2 and codepoint2 <= 0xDFFF)) + { + // overwrite codepoint + codepoint = + // high surrogate occupies the most significant 22 bits + (codepoint1 << 10) + // low surrogate occupies the least significant 15 bits + + codepoint2 + // there is still the 0xD800, 0xDC00 and 0x10000 noise + // in the result so we have to subtract with: + // (0xD800 << 10) + DC00 - 0x10000 = 0x35FDC00 + - 0x35FDC00; + } + else + { + error_message = "invalid string: surrogate U+DC00..U+DFFF must be followed by U+DC00..U+DFFF"; + return token_type::parse_error; + } + } + else + { + error_message = "invalid string: surrogate U+DC00..U+DFFF must be followed by U+DC00..U+DFFF"; + return token_type::parse_error; + } + } + else + { + if (JSON_UNLIKELY(0xDC00 <= codepoint1 and codepoint1 <= 0xDFFF)) + { + error_message = "invalid string: surrogate U+DC00..U+DFFF must follow U+D800..U+DBFF"; + return token_type::parse_error; + } + } + + // result of the above calculation yields a proper codepoint + assert(0x00 <= codepoint and codepoint <= 0x10FFFF); + + // translate codepoint into bytes + if (codepoint < 0x80) + { + // 1-byte characters: 0xxxxxxx (ASCII) + add(codepoint); + } + else if (codepoint <= 0x7FF) + { + // 2-byte characters: 110xxxxx 10xxxxxx + add(0xC0 | (codepoint >> 6)); + add(0x80 | (codepoint & 0x3F)); + } + else if (codepoint <= 0xFFFF) + { + // 3-byte characters: 1110xxxx 10xxxxxx 10xxxxxx + add(0xE0 | (codepoint >> 12)); + add(0x80 | ((codepoint >> 6) & 0x3F)); + add(0x80 | (codepoint & 0x3F)); + } + else + { + // 4-byte characters: 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx + add(0xF0 | (codepoint >> 18)); + add(0x80 | ((codepoint >> 12) & 0x3F)); + add(0x80 | ((codepoint >> 6) & 0x3F)); + add(0x80 | (codepoint & 0x3F)); + } + + break; + } + + // other characters after escape + default: + error_message = "invalid string: forbidden character after backslash"; + return token_type::parse_error; + } + + break; + } + + // invalid control characters + case 0x00: + { + error_message = "invalid string: control character U+0000 (NUL) must be escaped to \\u0000"; + return token_type::parse_error; + } + + case 0x01: + { + error_message = "invalid string: control character U+0001 (SOH) must be escaped to \\u0001"; + return token_type::parse_error; + } + + case 0x02: + { + error_message = "invalid string: control character U+0002 (STX) must be escaped to \\u0002"; + return token_type::parse_error; + } + + case 0x03: + { + error_message = "invalid string: control character U+0003 (ETX) must be escaped to \\u0003"; + return token_type::parse_error; + } + + case 0x04: + { + error_message = "invalid string: control character U+0004 (EOT) must be escaped to \\u0004"; + return token_type::parse_error; + } + + case 0x05: + { + error_message = "invalid string: control character U+0005 (ENQ) must be escaped to \\u0005"; + return token_type::parse_error; + } + + case 0x06: + { + error_message = "invalid string: control character U+0006 (ACK) must be escaped to \\u0006"; + return token_type::parse_error; + } + + case 0x07: + { + error_message = "invalid string: control character U+0007 (BEL) must be escaped to \\u0007"; + return token_type::parse_error; + } + + case 0x08: + { + error_message = "invalid string: control character U+0008 (BS) must be escaped to \\u0008 or \\b"; + return token_type::parse_error; + } + + case 0x09: + { + error_message = "invalid string: control character U+0009 (HT) must be escaped to \\u0009 or \\t"; + return token_type::parse_error; + } + + case 0x0A: + { + error_message = "invalid string: control character U+000A (LF) must be escaped to \\u000A or \\n"; + return token_type::parse_error; + } + + case 0x0B: + { + error_message = "invalid string: control character U+000B (VT) must be escaped to \\u000B"; + return token_type::parse_error; + } + + case 0x0C: + { + error_message = "invalid string: control character U+000C (FF) must be escaped to \\u000C or \\f"; + return token_type::parse_error; + } + + case 0x0D: + { + error_message = "invalid string: control character U+000D (CR) must be escaped to \\u000D or \\r"; + return token_type::parse_error; + } + + case 0x0E: + { + error_message = "invalid string: control character U+000E (SO) must be escaped to \\u000E"; + return token_type::parse_error; + } + + case 0x0F: + { + error_message = "invalid string: control character U+000F (SI) must be escaped to \\u000F"; + return token_type::parse_error; + } + + case 0x10: + { + error_message = "invalid string: control character U+0010 (DLE) must be escaped to \\u0010"; + return token_type::parse_error; + } + + case 0x11: + { + error_message = "invalid string: control character U+0011 (DC1) must be escaped to \\u0011"; + return token_type::parse_error; + } + + case 0x12: + { + error_message = "invalid string: control character U+0012 (DC2) must be escaped to \\u0012"; + return token_type::parse_error; + } + + case 0x13: + { + error_message = "invalid string: control character U+0013 (DC3) must be escaped to \\u0013"; + return token_type::parse_error; + } + + case 0x14: + { + error_message = "invalid string: control character U+0014 (DC4) must be escaped to \\u0014"; + return token_type::parse_error; + } + + case 0x15: + { + error_message = "invalid string: control character U+0015 (NAK) must be escaped to \\u0015"; + return token_type::parse_error; + } + + case 0x16: + { + error_message = "invalid string: control character U+0016 (SYN) must be escaped to \\u0016"; + return token_type::parse_error; + } + + case 0x17: + { + error_message = "invalid string: control character U+0017 (ETB) must be escaped to \\u0017"; + return token_type::parse_error; + } + + case 0x18: + { + error_message = "invalid string: control character U+0018 (CAN) must be escaped to \\u0018"; + return token_type::parse_error; + } + + case 0x19: + { + error_message = "invalid string: control character U+0019 (EM) must be escaped to \\u0019"; + return token_type::parse_error; + } + + case 0x1A: + { + error_message = "invalid string: control character U+001A (SUB) must be escaped to \\u001A"; + return token_type::parse_error; + } + + case 0x1B: + { + error_message = "invalid string: control character U+001B (ESC) must be escaped to \\u001B"; + return token_type::parse_error; + } + + case 0x1C: + { + error_message = "invalid string: control character U+001C (FS) must be escaped to \\u001C"; + return token_type::parse_error; + } + + case 0x1D: + { + error_message = "invalid string: control character U+001D (GS) must be escaped to \\u001D"; + return token_type::parse_error; + } + + case 0x1E: + { + error_message = "invalid string: control character U+001E (RS) must be escaped to \\u001E"; + return token_type::parse_error; + } + + case 0x1F: + { + error_message = "invalid string: control character U+001F (US) must be escaped to \\u001F"; + return token_type::parse_error; + } + + // U+0020..U+007F (except U+0022 (quote) and U+005C (backspace)) + case 0x20: + case 0x21: + case 0x23: + case 0x24: + case 0x25: + case 0x26: + case 0x27: + case 0x28: + case 0x29: + case 0x2A: + case 0x2B: + case 0x2C: + case 0x2D: + case 0x2E: + case 0x2F: + case 0x30: + case 0x31: + case 0x32: + case 0x33: + case 0x34: + case 0x35: + case 0x36: + case 0x37: + case 0x38: + case 0x39: + case 0x3A: + case 0x3B: + case 0x3C: + case 0x3D: + case 0x3E: + case 0x3F: + case 0x40: + case 0x41: + case 0x42: + case 0x43: + case 0x44: + case 0x45: + case 0x46: + case 0x47: + case 0x48: + case 0x49: + case 0x4A: + case 0x4B: + case 0x4C: + case 0x4D: + case 0x4E: + case 0x4F: + case 0x50: + case 0x51: + case 0x52: + case 0x53: + case 0x54: + case 0x55: + case 0x56: + case 0x57: + case 0x58: + case 0x59: + case 0x5A: + case 0x5B: + case 0x5D: + case 0x5E: + case 0x5F: + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + case 0x78: + case 0x79: + case 0x7A: + case 0x7B: + case 0x7C: + case 0x7D: + case 0x7E: + case 0x7F: + { + add(current); + break; + } + + // U+0080..U+07FF: bytes C2..DF 80..BF + case 0xC2: + case 0xC3: + case 0xC4: + case 0xC5: + case 0xC6: + case 0xC7: + case 0xC8: + case 0xC9: + case 0xCA: + case 0xCB: + case 0xCC: + case 0xCD: + case 0xCE: + case 0xCF: + case 0xD0: + case 0xD1: + case 0xD2: + case 0xD3: + case 0xD4: + case 0xD5: + case 0xD6: + case 0xD7: + case 0xD8: + case 0xD9: + case 0xDA: + case 0xDB: + case 0xDC: + case 0xDD: + case 0xDE: + case 0xDF: + { + if (JSON_UNLIKELY(not next_byte_in_range({0x80, 0xBF}))) + { + return token_type::parse_error; + } + break; + } + + // U+0800..U+0FFF: bytes E0 A0..BF 80..BF + case 0xE0: + { + if (JSON_UNLIKELY(not (next_byte_in_range({0xA0, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+1000..U+CFFF: bytes E1..EC 80..BF 80..BF + // U+E000..U+FFFF: bytes EE..EF 80..BF 80..BF + case 0xE1: + case 0xE2: + case 0xE3: + case 0xE4: + case 0xE5: + case 0xE6: + case 0xE7: + case 0xE8: + case 0xE9: + case 0xEA: + case 0xEB: + case 0xEC: + case 0xEE: + case 0xEF: + { + if (JSON_UNLIKELY(not (next_byte_in_range({0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+D000..U+D7FF: bytes ED 80..9F 80..BF + case 0xED: + { + if (JSON_UNLIKELY(not (next_byte_in_range({0x80, 0x9F, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+10000..U+3FFFF F0 90..BF 80..BF 80..BF + case 0xF0: + { + if (JSON_UNLIKELY(not (next_byte_in_range({0x90, 0xBF, 0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+40000..U+FFFFF F1..F3 80..BF 80..BF 80..BF + case 0xF1: + case 0xF2: + case 0xF3: + { + if (JSON_UNLIKELY(not (next_byte_in_range({0x80, 0xBF, 0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // U+100000..U+10FFFF F4 80..8F 80..BF 80..BF + case 0xF4: + { + if (JSON_UNLIKELY(not (next_byte_in_range({0x80, 0x8F, 0x80, 0xBF, 0x80, 0xBF})))) + { + return token_type::parse_error; + } + break; + } + + // remaining bytes (80..C1 and F5..FF) are ill-formed + default: + { + error_message = "invalid string: ill-formed UTF-8 byte"; + return token_type::parse_error; + } + } + } + } + + static void strtof(float& f, const char* str, char** endptr) noexcept + { + f = std::strtof(str, endptr); + } + + static void strtof(double& f, const char* str, char** endptr) noexcept + { + f = std::strtod(str, endptr); + } + + static void strtof(long double& f, const char* str, char** endptr) noexcept + { + f = std::strtold(str, endptr); + } + + /*! + @brief scan a number literal + + This function scans a string according to Sect. 6 of RFC 7159. + + The function is realized with a deterministic finite state machine derived + from the grammar described in RFC 7159. Starting in state "init", the + input is read and used to determined the next state. Only state "done" + accepts the number. State "error" is a trap state to model errors. In the + table below, "anything" means any character but the ones listed before. + + state | 0 | 1-9 | e E | + | - | . | anything + ---------|----------|----------|----------|---------|---------|----------|----------- + init | zero | any1 | [error] | [error] | minus | [error] | [error] + minus | zero | any1 | [error] | [error] | [error] | [error] | [error] + zero | done | done | exponent | done | done | decimal1 | done + any1 | any1 | any1 | exponent | done | done | decimal1 | done + decimal1 | decimal2 | [error] | [error] | [error] | [error] | [error] | [error] + decimal2 | decimal2 | decimal2 | exponent | done | done | done | done + exponent | any2 | any2 | [error] | sign | sign | [error] | [error] + sign | any2 | any2 | [error] | [error] | [error] | [error] | [error] + any2 | any2 | any2 | done | done | done | done | done + + The state machine is realized with one label per state (prefixed with + "scan_number_") and `goto` statements between them. The state machine + contains cycles, but any cycle can be left when EOF is read. Therefore, + the function is guaranteed to terminate. + + During scanning, the read bytes are stored in token_buffer. This string is + then converted to a signed integer, an unsigned integer, or a + floating-point number. + + @return token_type::value_unsigned, token_type::value_integer, or + token_type::value_float if number could be successfully scanned, + token_type::parse_error otherwise + + @note The scanner is independent of the current locale. Internally, the + locale's decimal point is used instead of `.` to work with the + locale-dependent converters. + */ + token_type scan_number() // lgtm [cpp/use-of-goto] + { + // reset token_buffer to store the number's bytes + reset(); + + // the type of the parsed number; initially set to unsigned; will be + // changed if minus sign, decimal point or exponent is read + token_type number_type = token_type::value_unsigned; + + // state (init): we just found out we need to scan a number + switch (current) + { + case '-': + { + add(current); + goto scan_number_minus; + } + + case '0': + { + add(current); + goto scan_number_zero; + } + + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any1; + } + + // LCOV_EXCL_START + default: + { + // all other characters are rejected outside scan_number() + assert(false); + } + // LCOV_EXCL_STOP + } + +scan_number_minus: + // state: we just parsed a leading minus sign + number_type = token_type::value_integer; + switch (get()) + { + case '0': + { + add(current); + goto scan_number_zero; + } + + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any1; + } + + default: + { + error_message = "invalid number; expected digit after '-'"; + return token_type::parse_error; + } + } + +scan_number_zero: + // state: we just parse a zero (maybe with a leading minus sign) + switch (get()) + { + case '.': + { + add(decimal_point_char); + goto scan_number_decimal1; + } + + case 'e': + case 'E': + { + add(current); + goto scan_number_exponent; + } + + default: + goto scan_number_done; + } + +scan_number_any1: + // state: we just parsed a number 0-9 (maybe with a leading minus sign) + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any1; + } + + case '.': + { + add(decimal_point_char); + goto scan_number_decimal1; + } + + case 'e': + case 'E': + { + add(current); + goto scan_number_exponent; + } + + default: + goto scan_number_done; + } + +scan_number_decimal1: + // state: we just parsed a decimal point + number_type = token_type::value_float; + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_decimal2; + } + + default: + { + error_message = "invalid number; expected digit after '.'"; + return token_type::parse_error; + } + } + +scan_number_decimal2: + // we just parsed at least one number after a decimal point + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_decimal2; + } + + case 'e': + case 'E': + { + add(current); + goto scan_number_exponent; + } + + default: + goto scan_number_done; + } + +scan_number_exponent: + // we just parsed an exponent + number_type = token_type::value_float; + switch (get()) + { + case '+': + case '-': + { + add(current); + goto scan_number_sign; + } + + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any2; + } + + default: + { + error_message = + "invalid number; expected '+', '-', or digit after exponent"; + return token_type::parse_error; + } + } + +scan_number_sign: + // we just parsed an exponent sign + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any2; + } + + default: + { + error_message = "invalid number; expected digit after exponent sign"; + return token_type::parse_error; + } + } + +scan_number_any2: + // we just parsed a number after the exponent or exponent sign + switch (get()) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + { + add(current); + goto scan_number_any2; + } + + default: + goto scan_number_done; + } + +scan_number_done: + // unget the character after the number (we only read it to know that + // we are done scanning a number) + unget(); + + char* endptr = nullptr; + errno = 0; + + // try to parse integers first and fall back to floats + if (number_type == token_type::value_unsigned) + { + const auto x = std::strtoull(token_buffer.data(), &endptr, 10); + + // we checked the number format before + assert(endptr == token_buffer.data() + token_buffer.size()); + + if (errno == 0) + { + value_unsigned = static_cast<number_unsigned_t>(x); + if (value_unsigned == x) + { + return token_type::value_unsigned; + } + } + } + else if (number_type == token_type::value_integer) + { + const auto x = std::strtoll(token_buffer.data(), &endptr, 10); + + // we checked the number format before + assert(endptr == token_buffer.data() + token_buffer.size()); + + if (errno == 0) + { + value_integer = static_cast<number_integer_t>(x); + if (value_integer == x) + { + return token_type::value_integer; + } + } + } + + // this code is reached if we parse a floating-point number or if an + // integer conversion above failed + strtof(value_float, token_buffer.data(), &endptr); + + // we checked the number format before + assert(endptr == token_buffer.data() + token_buffer.size()); + + return token_type::value_float; + } + + /*! + @param[in] literal_text the literal text to expect + @param[in] length the length of the passed literal text + @param[in] return_type the token type to return on success + */ + token_type scan_literal(const char* literal_text, const std::size_t length, + token_type return_type) + { + assert(current == literal_text[0]); + for (std::size_t i = 1; i < length; ++i) + { + if (JSON_UNLIKELY(get() != literal_text[i])) + { + error_message = "invalid literal"; + return token_type::parse_error; + } + } + return return_type; + } + + ///////////////////// + // input management + ///////////////////// + + /// reset token_buffer; current character is beginning of token + void reset() noexcept + { + token_buffer.clear(); + token_string.clear(); + token_string.push_back(std::char_traits<char>::to_char_type(current)); + } + + /* + @brief get next character from the input + + This function provides the interface to the used input adapter. It does + not throw in case the input reached EOF, but returns a + `std::char_traits<char>::eof()` in that case. Stores the scanned characters + for use in error messages. + + @return character read from the input + */ + std::char_traits<char>::int_type get() + { + ++position.chars_read_total; + ++position.chars_read_current_line; + + if (next_unget) + { + // just reset the next_unget variable and work with current + next_unget = false; + } + else + { + current = ia->get_character(); + } + + if (JSON_LIKELY(current != std::char_traits<char>::eof())) + { + token_string.push_back(std::char_traits<char>::to_char_type(current)); + } + + if (current == '\n') + { + ++position.lines_read; + ++position.chars_read_current_line = 0; + } + + return current; + } + + /*! + @brief unget current character (read it again on next get) + + We implement unget by setting variable next_unget to true. The input is not + changed - we just simulate ungetting by modifying chars_read_total, + chars_read_current_line, and token_string. The next call to get() will + behave as if the unget character is read again. + */ + void unget() + { + next_unget = true; + + --position.chars_read_total; + + // in case we "unget" a newline, we have to also decrement the lines_read + if (position.chars_read_current_line == 0) + { + if (position.lines_read > 0) + { + --position.lines_read; + } + } + else + { + --position.chars_read_current_line; + } + + if (JSON_LIKELY(current != std::char_traits<char>::eof())) + { + assert(token_string.size() != 0); + token_string.pop_back(); + } + } + + /// add a character to token_buffer + void add(int c) + { + token_buffer.push_back(std::char_traits<char>::to_char_type(c)); + } + + public: + ///////////////////// + // value getters + ///////////////////// + + /// return integer value + constexpr number_integer_t get_number_integer() const noexcept + { + return value_integer; + } + + /// return unsigned integer value + constexpr number_unsigned_t get_number_unsigned() const noexcept + { + return value_unsigned; + } + + /// return floating-point value + constexpr number_float_t get_number_float() const noexcept + { + return value_float; + } + + /// return current string value (implicitly resets the token; useful only once) + string_t& get_string() + { + return token_buffer; + } + + ///////////////////// + // diagnostics + ///////////////////// + + /// return position of last read token + constexpr position_t get_position() const noexcept + { + return position; + } + + /// return the last read token (for errors only). Will never contain EOF + /// (an arbitrary value that is not a valid char value, often -1), because + /// 255 may legitimately occur. May contain NUL, which should be escaped. + std::string get_token_string() const + { + // escape control characters + std::string result; + for (const auto c : token_string) + { + if ('\x00' <= c and c <= '\x1F') + { + // escape control characters + char cs[9]; + snprintf(cs, 9, "<U+%.4X>", static_cast<unsigned char>(c)); + result += cs; + } + else + { + // add character as is + result.push_back(c); + } + } + + return result; + } + + /// return syntax error message + constexpr const char* get_error_message() const noexcept + { + return error_message; + } + + ///////////////////// + // actual scanner + ///////////////////// + + /*! + @brief skip the UTF-8 byte order mark + @return true iff there is no BOM or the correct BOM has been skipped + */ + bool skip_bom() + { + if (get() == 0xEF) + { + // check if we completely parse the BOM + return get() == 0xBB and get() == 0xBF; + } + + // the first character is not the beginning of the BOM; unget it to + // process is later + unget(); + return true; + } + + token_type scan() + { + // initially, skip the BOM + if (position.chars_read_total == 0 and not skip_bom()) + { + error_message = "invalid BOM; must be 0xEF 0xBB 0xBF if given"; + return token_type::parse_error; + } + + // read next character and ignore whitespace + do + { + get(); + } + while (current == ' ' or current == '\t' or current == '\n' or current == '\r'); + + switch (current) + { + // structural characters + case '[': + return token_type::begin_array; + case ']': + return token_type::end_array; + case '{': + return token_type::begin_object; + case '}': + return token_type::end_object; + case ':': + return token_type::name_separator; + case ',': + return token_type::value_separator; + + // literals + case 't': + return scan_literal("true", 4, token_type::literal_true); + case 'f': + return scan_literal("false", 5, token_type::literal_false); + case 'n': + return scan_literal("null", 4, token_type::literal_null); + + // string + case '\"': + return scan_string(); + + // number + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + return scan_number(); + + // end of input (the null byte is needed when parsing from + // string literals) + case '\0': + case std::char_traits<char>::eof(): + return token_type::end_of_input; + + // error + default: + error_message = "invalid literal"; + return token_type::parse_error; + } + } + + private: + /// input adapter + detail::input_adapter_t ia = nullptr; + + /// the current character + std::char_traits<char>::int_type current = std::char_traits<char>::eof(); + + /// whether the next get() call should just return current + bool next_unget = false; + + /// the start position of the current token + position_t position; + + /// raw input token string (for error messages) + std::vector<char> token_string {}; + + /// buffer for variable-length tokens (numbers, strings) + string_t token_buffer {}; + + /// a description of occurred lexer errors + const char* error_message = ""; + + // number values + number_integer_t value_integer = 0; + number_unsigned_t value_unsigned = 0; + number_float_t value_float = 0; + + /// the decimal point + const char decimal_point_char = '.'; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/input/parser.hpp> + + +#include <cassert> // assert +#include <cmath> // isfinite +#include <cstdint> // uint8_t +#include <functional> // function +#include <string> // string +#include <utility> // move + +// #include <nlohmann/detail/exceptions.hpp> + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/meta/is_sax.hpp> + + +#include <cstdint> // size_t +#include <utility> // declval + +// #include <nlohmann/detail/meta/detected.hpp> + +// #include <nlohmann/detail/meta/type_traits.hpp> + + +namespace nlohmann +{ +namespace detail +{ +template <typename T> +using null_function_t = decltype(std::declval<T&>().null()); + +template <typename T> +using boolean_function_t = + decltype(std::declval<T&>().boolean(std::declval<bool>())); + +template <typename T, typename Integer> +using number_integer_function_t = + decltype(std::declval<T&>().number_integer(std::declval<Integer>())); + +template <typename T, typename Unsigned> +using number_unsigned_function_t = + decltype(std::declval<T&>().number_unsigned(std::declval<Unsigned>())); + +template <typename T, typename Float, typename String> +using number_float_function_t = decltype(std::declval<T&>().number_float( + std::declval<Float>(), std::declval<const String&>())); + +template <typename T, typename String> +using string_function_t = + decltype(std::declval<T&>().string(std::declval<String&>())); + +template <typename T> +using start_object_function_t = + decltype(std::declval<T&>().start_object(std::declval<std::size_t>())); + +template <typename T, typename String> +using key_function_t = + decltype(std::declval<T&>().key(std::declval<String&>())); + +template <typename T> +using end_object_function_t = decltype(std::declval<T&>().end_object()); + +template <typename T> +using start_array_function_t = + decltype(std::declval<T&>().start_array(std::declval<std::size_t>())); + +template <typename T> +using end_array_function_t = decltype(std::declval<T&>().end_array()); + +template <typename T, typename Exception> +using parse_error_function_t = decltype(std::declval<T&>().parse_error( + std::declval<std::size_t>(), std::declval<const std::string&>(), + std::declval<const Exception&>())); + +template <typename SAX, typename BasicJsonType> +struct is_sax +{ + private: + static_assert(is_basic_json<BasicJsonType>::value, + "BasicJsonType must be of type basic_json<...>"); + + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using exception_t = typename BasicJsonType::exception; + + public: + static constexpr bool value = + is_detected_exact<bool, null_function_t, SAX>::value && + is_detected_exact<bool, boolean_function_t, SAX>::value && + is_detected_exact<bool, number_integer_function_t, SAX, + number_integer_t>::value && + is_detected_exact<bool, number_unsigned_function_t, SAX, + number_unsigned_t>::value && + is_detected_exact<bool, number_float_function_t, SAX, number_float_t, + string_t>::value && + is_detected_exact<bool, string_function_t, SAX, string_t>::value && + is_detected_exact<bool, start_object_function_t, SAX>::value && + is_detected_exact<bool, key_function_t, SAX, string_t>::value && + is_detected_exact<bool, end_object_function_t, SAX>::value && + is_detected_exact<bool, start_array_function_t, SAX>::value && + is_detected_exact<bool, end_array_function_t, SAX>::value && + is_detected_exact<bool, parse_error_function_t, SAX, exception_t>::value; +}; + +template <typename SAX, typename BasicJsonType> +struct is_sax_static_asserts +{ + private: + static_assert(is_basic_json<BasicJsonType>::value, + "BasicJsonType must be of type basic_json<...>"); + + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using exception_t = typename BasicJsonType::exception; + + public: + static_assert(is_detected_exact<bool, null_function_t, SAX>::value, + "Missing/invalid function: bool null()"); + static_assert(is_detected_exact<bool, boolean_function_t, SAX>::value, + "Missing/invalid function: bool boolean(bool)"); + static_assert(is_detected_exact<bool, boolean_function_t, SAX>::value, + "Missing/invalid function: bool boolean(bool)"); + static_assert( + is_detected_exact<bool, number_integer_function_t, SAX, + number_integer_t>::value, + "Missing/invalid function: bool number_integer(number_integer_t)"); + static_assert( + is_detected_exact<bool, number_unsigned_function_t, SAX, + number_unsigned_t>::value, + "Missing/invalid function: bool number_unsigned(number_unsigned_t)"); + static_assert(is_detected_exact<bool, number_float_function_t, SAX, + number_float_t, string_t>::value, + "Missing/invalid function: bool number_float(number_float_t, const string_t&)"); + static_assert( + is_detected_exact<bool, string_function_t, SAX, string_t>::value, + "Missing/invalid function: bool string(string_t&)"); + static_assert(is_detected_exact<bool, start_object_function_t, SAX>::value, + "Missing/invalid function: bool start_object(std::size_t)"); + static_assert(is_detected_exact<bool, key_function_t, SAX, string_t>::value, + "Missing/invalid function: bool key(string_t&)"); + static_assert(is_detected_exact<bool, end_object_function_t, SAX>::value, + "Missing/invalid function: bool end_object()"); + static_assert(is_detected_exact<bool, start_array_function_t, SAX>::value, + "Missing/invalid function: bool start_array(std::size_t)"); + static_assert(is_detected_exact<bool, end_array_function_t, SAX>::value, + "Missing/invalid function: bool end_array()"); + static_assert( + is_detected_exact<bool, parse_error_function_t, SAX, exception_t>::value, + "Missing/invalid function: bool parse_error(std::size_t, const " + "std::string&, const exception&)"); +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/input/input_adapters.hpp> + +// #include <nlohmann/detail/input/json_sax.hpp> + + +#include <cstddef> +#include <string> +#include <vector> + +// #include <nlohmann/detail/input/parser.hpp> + +// #include <nlohmann/detail/exceptions.hpp> + + +namespace nlohmann +{ + +/*! +@brief SAX interface + +This class describes the SAX interface used by @ref nlohmann::json::sax_parse. +Each function is called in different situations while the input is parsed. The +boolean return value informs the parser whether to continue processing the +input. +*/ +template<typename BasicJsonType> +struct json_sax +{ + /// type for (signed) integers + using number_integer_t = typename BasicJsonType::number_integer_t; + /// type for unsigned integers + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + /// type for floating-point numbers + using number_float_t = typename BasicJsonType::number_float_t; + /// type for strings + using string_t = typename BasicJsonType::string_t; + + /*! + @brief a null value was read + @return whether parsing should proceed + */ + virtual bool null() = 0; + + /*! + @brief a boolean value was read + @param[in] val boolean value + @return whether parsing should proceed + */ + virtual bool boolean(bool val) = 0; + + /*! + @brief an integer number was read + @param[in] val integer value + @return whether parsing should proceed + */ + virtual bool number_integer(number_integer_t val) = 0; + + /*! + @brief an unsigned integer number was read + @param[in] val unsigned integer value + @return whether parsing should proceed + */ + virtual bool number_unsigned(number_unsigned_t val) = 0; + + /*! + @brief an floating-point number was read + @param[in] val floating-point value + @param[in] s raw token value + @return whether parsing should proceed + */ + virtual bool number_float(number_float_t val, const string_t& s) = 0; + + /*! + @brief a string was read + @param[in] val string value + @return whether parsing should proceed + @note It is safe to move the passed string. + */ + virtual bool string(string_t& val) = 0; + + /*! + @brief the beginning of an object was read + @param[in] elements number of object elements or -1 if unknown + @return whether parsing should proceed + @note binary formats may report the number of elements + */ + virtual bool start_object(std::size_t elements) = 0; + + /*! + @brief an object key was read + @param[in] val object key + @return whether parsing should proceed + @note It is safe to move the passed string. + */ + virtual bool key(string_t& val) = 0; + + /*! + @brief the end of an object was read + @return whether parsing should proceed + */ + virtual bool end_object() = 0; + + /*! + @brief the beginning of an array was read + @param[in] elements number of array elements or -1 if unknown + @return whether parsing should proceed + @note binary formats may report the number of elements + */ + virtual bool start_array(std::size_t elements) = 0; + + /*! + @brief the end of an array was read + @return whether parsing should proceed + */ + virtual bool end_array() = 0; + + /*! + @brief a parse error occurred + @param[in] position the position in the input where the error occurs + @param[in] last_token the last read token + @param[in] ex an exception object describing the error + @return whether parsing should proceed (must return false) + */ + virtual bool parse_error(std::size_t position, + const std::string& last_token, + const detail::exception& ex) = 0; + + virtual ~json_sax() = default; +}; + + +namespace detail +{ +/*! +@brief SAX implementation to create a JSON value from SAX events + +This class implements the @ref json_sax interface and processes the SAX events +to create a JSON value which makes it basically a DOM parser. The structure or +hierarchy of the JSON value is managed by the stack `ref_stack` which contains +a pointer to the respective array or object for each recursion depth. + +After successful parsing, the value that is passed by reference to the +constructor contains the parsed value. + +@tparam BasicJsonType the JSON type +*/ +template<typename BasicJsonType> +class json_sax_dom_parser +{ + public: + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + + /*! + @param[in, out] r reference to a JSON value that is manipulated while + parsing + @param[in] allow_exceptions_ whether parse errors yield exceptions + */ + explicit json_sax_dom_parser(BasicJsonType& r, const bool allow_exceptions_ = true) + : root(r), allow_exceptions(allow_exceptions_) + {} + + bool null() + { + handle_value(nullptr); + return true; + } + + bool boolean(bool val) + { + handle_value(val); + return true; + } + + bool number_integer(number_integer_t val) + { + handle_value(val); + return true; + } + + bool number_unsigned(number_unsigned_t val) + { + handle_value(val); + return true; + } + + bool number_float(number_float_t val, const string_t& /*unused*/) + { + handle_value(val); + return true; + } + + bool string(string_t& val) + { + handle_value(val); + return true; + } + + bool start_object(std::size_t len) + { + ref_stack.push_back(handle_value(BasicJsonType::value_t::object)); + + if (JSON_UNLIKELY(len != std::size_t(-1) and len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, + "excessive object size: " + std::to_string(len))); + } + + return true; + } + + bool key(string_t& val) + { + // add null at given key and store the reference for later + object_element = &(ref_stack.back()->m_value.object->operator[](val)); + return true; + } + + bool end_object() + { + ref_stack.pop_back(); + return true; + } + + bool start_array(std::size_t len) + { + ref_stack.push_back(handle_value(BasicJsonType::value_t::array)); + + if (JSON_UNLIKELY(len != std::size_t(-1) and len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, + "excessive array size: " + std::to_string(len))); + } + + return true; + } + + bool end_array() + { + ref_stack.pop_back(); + return true; + } + + bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, + const detail::exception& ex) + { + errored = true; + if (allow_exceptions) + { + // determine the proper exception type from the id + switch ((ex.id / 100) % 100) + { + case 1: + JSON_THROW(*reinterpret_cast<const detail::parse_error*>(&ex)); + case 4: + JSON_THROW(*reinterpret_cast<const detail::out_of_range*>(&ex)); + // LCOV_EXCL_START + case 2: + JSON_THROW(*reinterpret_cast<const detail::invalid_iterator*>(&ex)); + case 3: + JSON_THROW(*reinterpret_cast<const detail::type_error*>(&ex)); + case 5: + JSON_THROW(*reinterpret_cast<const detail::other_error*>(&ex)); + default: + assert(false); + // LCOV_EXCL_STOP + } + } + return false; + } + + constexpr bool is_errored() const + { + return errored; + } + + private: + /*! + @invariant If the ref stack is empty, then the passed value will be the new + root. + @invariant If the ref stack contains a value, then it is an array or an + object to which we can add elements + */ + template<typename Value> + BasicJsonType* handle_value(Value&& v) + { + if (ref_stack.empty()) + { + root = BasicJsonType(std::forward<Value>(v)); + return &root; + } + + assert(ref_stack.back()->is_array() or ref_stack.back()->is_object()); + + if (ref_stack.back()->is_array()) + { + ref_stack.back()->m_value.array->emplace_back(std::forward<Value>(v)); + return &(ref_stack.back()->m_value.array->back()); + } + else + { + assert(object_element); + *object_element = BasicJsonType(std::forward<Value>(v)); + return object_element; + } + } + + /// the parsed JSON value + BasicJsonType& root; + /// stack to model hierarchy of values + std::vector<BasicJsonType*> ref_stack; + /// helper to hold the reference for the next object element + BasicJsonType* object_element = nullptr; + /// whether a syntax error occurred + bool errored = false; + /// whether to throw exceptions in case of errors + const bool allow_exceptions = true; +}; + +template<typename BasicJsonType> +class json_sax_dom_callback_parser +{ + public: + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using parser_callback_t = typename BasicJsonType::parser_callback_t; + using parse_event_t = typename BasicJsonType::parse_event_t; + + json_sax_dom_callback_parser(BasicJsonType& r, + const parser_callback_t cb, + const bool allow_exceptions_ = true) + : root(r), callback(cb), allow_exceptions(allow_exceptions_) + { + keep_stack.push_back(true); + } + + bool null() + { + handle_value(nullptr); + return true; + } + + bool boolean(bool val) + { + handle_value(val); + return true; + } + + bool number_integer(number_integer_t val) + { + handle_value(val); + return true; + } + + bool number_unsigned(number_unsigned_t val) + { + handle_value(val); + return true; + } + + bool number_float(number_float_t val, const string_t& /*unused*/) + { + handle_value(val); + return true; + } + + bool string(string_t& val) + { + handle_value(val); + return true; + } + + bool start_object(std::size_t len) + { + // check callback for object start + const bool keep = callback(static_cast<int>(ref_stack.size()), parse_event_t::object_start, discarded); + keep_stack.push_back(keep); + + auto val = handle_value(BasicJsonType::value_t::object, true); + ref_stack.push_back(val.second); + + // check object limit + if (ref_stack.back()) + { + if (JSON_UNLIKELY(len != std::size_t(-1) and len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, + "excessive object size: " + std::to_string(len))); + } + } + + return true; + } + + bool key(string_t& val) + { + BasicJsonType k = BasicJsonType(val); + + // check callback for key + const bool keep = callback(static_cast<int>(ref_stack.size()), parse_event_t::key, k); + key_keep_stack.push_back(keep); + + // add discarded value at given key and store the reference for later + if (keep and ref_stack.back()) + { + object_element = &(ref_stack.back()->m_value.object->operator[](val) = discarded); + } + + return true; + } + + bool end_object() + { + if (ref_stack.back()) + { + if (not callback(static_cast<int>(ref_stack.size()) - 1, parse_event_t::object_end, *ref_stack.back())) + { + // discard object + *ref_stack.back() = discarded; + } + } + + assert(not ref_stack.empty()); + assert(not keep_stack.empty()); + ref_stack.pop_back(); + keep_stack.pop_back(); + + if (not ref_stack.empty() and ref_stack.back()) + { + // remove discarded value + if (ref_stack.back()->is_object()) + { + for (auto it = ref_stack.back()->begin(); it != ref_stack.back()->end(); ++it) + { + if (it->is_discarded()) + { + ref_stack.back()->erase(it); + break; + } + } + } + } + + return true; + } + + bool start_array(std::size_t len) + { + const bool keep = callback(static_cast<int>(ref_stack.size()), parse_event_t::array_start, discarded); + keep_stack.push_back(keep); + + auto val = handle_value(BasicJsonType::value_t::array, true); + ref_stack.push_back(val.second); + + // check array limit + if (ref_stack.back()) + { + if (JSON_UNLIKELY(len != std::size_t(-1) and len > ref_stack.back()->max_size())) + { + JSON_THROW(out_of_range::create(408, + "excessive array size: " + std::to_string(len))); + } + } + + return true; + } + + bool end_array() + { + bool keep = true; + + if (ref_stack.back()) + { + keep = callback(static_cast<int>(ref_stack.size()) - 1, parse_event_t::array_end, *ref_stack.back()); + if (not keep) + { + // discard array + *ref_stack.back() = discarded; + } + } + + assert(not ref_stack.empty()); + assert(not keep_stack.empty()); + ref_stack.pop_back(); + keep_stack.pop_back(); + + // remove discarded value + if (not keep and not ref_stack.empty()) + { + if (ref_stack.back()->is_array()) + { + ref_stack.back()->m_value.array->pop_back(); + } + } + + return true; + } + + bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, + const detail::exception& ex) + { + errored = true; + if (allow_exceptions) + { + // determine the proper exception type from the id + switch ((ex.id / 100) % 100) + { + case 1: + JSON_THROW(*reinterpret_cast<const detail::parse_error*>(&ex)); + case 4: + JSON_THROW(*reinterpret_cast<const detail::out_of_range*>(&ex)); + // LCOV_EXCL_START + case 2: + JSON_THROW(*reinterpret_cast<const detail::invalid_iterator*>(&ex)); + case 3: + JSON_THROW(*reinterpret_cast<const detail::type_error*>(&ex)); + case 5: + JSON_THROW(*reinterpret_cast<const detail::other_error*>(&ex)); + default: + assert(false); + // LCOV_EXCL_STOP + } + } + return false; + } + + constexpr bool is_errored() const + { + return errored; + } + + private: + /*! + @param[in] v value to add to the JSON value we build during parsing + @param[in] skip_callback whether we should skip calling the callback + function; this is required after start_array() and + start_object() SAX events, because otherwise we would call the + callback function with an empty array or object, respectively. + + @invariant If the ref stack is empty, then the passed value will be the new + root. + @invariant If the ref stack contains a value, then it is an array or an + object to which we can add elements + + @return pair of boolean (whether value should be kept) and pointer (to the + passed value in the ref_stack hierarchy; nullptr if not kept) + */ + template<typename Value> + std::pair<bool, BasicJsonType*> handle_value(Value&& v, const bool skip_callback = false) + { + assert(not keep_stack.empty()); + + // do not handle this value if we know it would be added to a discarded + // container + if (not keep_stack.back()) + { + return {false, nullptr}; + } + + // create value + auto value = BasicJsonType(std::forward<Value>(v)); + + // check callback + const bool keep = skip_callback or callback(static_cast<int>(ref_stack.size()), parse_event_t::value, value); + + // do not handle this value if we just learnt it shall be discarded + if (not keep) + { + return {false, nullptr}; + } + + if (ref_stack.empty()) + { + root = std::move(value); + return {true, &root}; + } + + // skip this value if we already decided to skip the parent + // (https://github.com/nlohmann/json/issues/971#issuecomment-413678360) + if (not ref_stack.back()) + { + return {false, nullptr}; + } + + // we now only expect arrays and objects + assert(ref_stack.back()->is_array() or ref_stack.back()->is_object()); + + if (ref_stack.back()->is_array()) + { + ref_stack.back()->m_value.array->push_back(std::move(value)); + return {true, &(ref_stack.back()->m_value.array->back())}; + } + else + { + // check if we should store an element for the current key + assert(not key_keep_stack.empty()); + const bool store_element = key_keep_stack.back(); + key_keep_stack.pop_back(); + + if (not store_element) + { + return {false, nullptr}; + } + + assert(object_element); + *object_element = std::move(value); + return {true, object_element}; + } + } + + /// the parsed JSON value + BasicJsonType& root; + /// stack to model hierarchy of values + std::vector<BasicJsonType*> ref_stack; + /// stack to manage which values to keep + std::vector<bool> keep_stack; + /// stack to manage which object keys to keep + std::vector<bool> key_keep_stack; + /// helper to hold the reference for the next object element + BasicJsonType* object_element = nullptr; + /// whether a syntax error occurred + bool errored = false; + /// callback function + const parser_callback_t callback = nullptr; + /// whether to throw exceptions in case of errors + const bool allow_exceptions = true; + /// a discarded value for the callback + BasicJsonType discarded = BasicJsonType::value_t::discarded; +}; + +template<typename BasicJsonType> +class json_sax_acceptor +{ + public: + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + + bool null() + { + return true; + } + + bool boolean(bool /*unused*/) + { + return true; + } + + bool number_integer(number_integer_t /*unused*/) + { + return true; + } + + bool number_unsigned(number_unsigned_t /*unused*/) + { + return true; + } + + bool number_float(number_float_t /*unused*/, const string_t& /*unused*/) + { + return true; + } + + bool string(string_t& /*unused*/) + { + return true; + } + + bool start_object(std::size_t /*unused*/ = std::size_t(-1)) + { + return true; + } + + bool key(string_t& /*unused*/) + { + return true; + } + + bool end_object() + { + return true; + } + + bool start_array(std::size_t /*unused*/ = std::size_t(-1)) + { + return true; + } + + bool end_array() + { + return true; + } + + bool parse_error(std::size_t /*unused*/, const std::string& /*unused*/, const detail::exception& /*unused*/) + { + return false; + } +}; +} // namespace detail + +} // namespace nlohmann + +// #include <nlohmann/detail/input/lexer.hpp> + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +//////////// +// parser // +//////////// + +/*! +@brief syntax analysis + +This class implements a recursive decent parser. +*/ +template<typename BasicJsonType> +class parser +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using lexer_t = lexer<BasicJsonType>; + using token_type = typename lexer_t::token_type; + + public: + enum class parse_event_t : uint8_t + { + /// the parser read `{` and started to process a JSON object + object_start, + /// the parser read `}` and finished processing a JSON object + object_end, + /// the parser read `[` and started to process a JSON array + array_start, + /// the parser read `]` and finished processing a JSON array + array_end, + /// the parser read a key of a value in an object + key, + /// the parser finished reading a JSON value + value + }; + + using parser_callback_t = + std::function<bool(int depth, parse_event_t event, BasicJsonType& parsed)>; + + /// a parser reading from an input adapter + explicit parser(detail::input_adapter_t&& adapter, + const parser_callback_t cb = nullptr, + const bool allow_exceptions_ = true) + : callback(cb), m_lexer(std::move(adapter)), allow_exceptions(allow_exceptions_) + { + // read first token + get_token(); + } + + /*! + @brief public parser interface + + @param[in] strict whether to expect the last token to be EOF + @param[in,out] result parsed JSON value + + @throw parse_error.101 in case of an unexpected token + @throw parse_error.102 if to_unicode fails or surrogate error + @throw parse_error.103 if to_unicode fails + */ + void parse(const bool strict, BasicJsonType& result) + { + if (callback) + { + json_sax_dom_callback_parser<BasicJsonType> sdp(result, callback, allow_exceptions); + sax_parse_internal(&sdp); + result.assert_invariant(); + + // in strict mode, input must be completely read + if (strict and (get_token() != token_type::end_of_input)) + { + sdp.parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::end_of_input, "value"))); + } + + // in case of an error, return discarded value + if (sdp.is_errored()) + { + result = value_t::discarded; + return; + } + + // set top-level value to null if it was discarded by the callback + // function + if (result.is_discarded()) + { + result = nullptr; + } + } + else + { + json_sax_dom_parser<BasicJsonType> sdp(result, allow_exceptions); + sax_parse_internal(&sdp); + result.assert_invariant(); + + // in strict mode, input must be completely read + if (strict and (get_token() != token_type::end_of_input)) + { + sdp.parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::end_of_input, "value"))); + } + + // in case of an error, return discarded value + if (sdp.is_errored()) + { + result = value_t::discarded; + return; + } + } + } + + /*! + @brief public accept interface + + @param[in] strict whether to expect the last token to be EOF + @return whether the input is a proper JSON text + */ + bool accept(const bool strict = true) + { + json_sax_acceptor<BasicJsonType> sax_acceptor; + return sax_parse(&sax_acceptor, strict); + } + + template <typename SAX> + bool sax_parse(SAX* sax, const bool strict = true) + { + (void)detail::is_sax_static_asserts<SAX, BasicJsonType> {}; + const bool result = sax_parse_internal(sax); + + // strict mode: next byte must be EOF + if (result and strict and (get_token() != token_type::end_of_input)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::end_of_input, "value"))); + } + + return result; + } + + private: + template <typename SAX> + bool sax_parse_internal(SAX* sax) + { + // stack to remember the hierarchy of structured values we are parsing + // true = array; false = object + std::vector<bool> states; + // value to avoid a goto (see comment where set to true) + bool skip_to_state_evaluation = false; + + while (true) + { + if (not skip_to_state_evaluation) + { + // invariant: get_token() was called before each iteration + switch (last_token) + { + case token_type::begin_object: + { + if (JSON_UNLIKELY(not sax->start_object(std::size_t(-1)))) + { + return false; + } + + // closing } -> we are done + if (get_token() == token_type::end_object) + { + if (JSON_UNLIKELY(not sax->end_object())) + { + return false; + } + break; + } + + // parse key + if (JSON_UNLIKELY(last_token != token_type::value_string)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::value_string, "object key"))); + } + if (JSON_UNLIKELY(not sax->key(m_lexer.get_string()))) + { + return false; + } + + // parse separator (:) + if (JSON_UNLIKELY(get_token() != token_type::name_separator)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::name_separator, "object separator"))); + } + + // remember we are now inside an object + states.push_back(false); + + // parse values + get_token(); + continue; + } + + case token_type::begin_array: + { + if (JSON_UNLIKELY(not sax->start_array(std::size_t(-1)))) + { + return false; + } + + // closing ] -> we are done + if (get_token() == token_type::end_array) + { + if (JSON_UNLIKELY(not sax->end_array())) + { + return false; + } + break; + } + + // remember we are now inside an array + states.push_back(true); + + // parse values (no need to call get_token) + continue; + } + + case token_type::value_float: + { + const auto res = m_lexer.get_number_float(); + + if (JSON_UNLIKELY(not std::isfinite(res))) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + out_of_range::create(406, "number overflow parsing '" + m_lexer.get_token_string() + "'")); + } + else + { + if (JSON_UNLIKELY(not sax->number_float(res, m_lexer.get_string()))) + { + return false; + } + break; + } + } + + case token_type::literal_false: + { + if (JSON_UNLIKELY(not sax->boolean(false))) + { + return false; + } + break; + } + + case token_type::literal_null: + { + if (JSON_UNLIKELY(not sax->null())) + { + return false; + } + break; + } + + case token_type::literal_true: + { + if (JSON_UNLIKELY(not sax->boolean(true))) + { + return false; + } + break; + } + + case token_type::value_integer: + { + if (JSON_UNLIKELY(not sax->number_integer(m_lexer.get_number_integer()))) + { + return false; + } + break; + } + + case token_type::value_string: + { + if (JSON_UNLIKELY(not sax->string(m_lexer.get_string()))) + { + return false; + } + break; + } + + case token_type::value_unsigned: + { + if (JSON_UNLIKELY(not sax->number_unsigned(m_lexer.get_number_unsigned()))) + { + return false; + } + break; + } + + case token_type::parse_error: + { + // using "uninitialized" to avoid "expected" message + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::uninitialized, "value"))); + } + + default: // the last token was unexpected + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::literal_or_value, "value"))); + } + } + } + else + { + skip_to_state_evaluation = false; + } + + // we reached this line after we successfully parsed a value + if (states.empty()) + { + // empty stack: we reached the end of the hierarchy: done + return true; + } + else + { + if (states.back()) // array + { + // comma -> next value + if (get_token() == token_type::value_separator) + { + // parse a new value + get_token(); + continue; + } + + // closing ] + if (JSON_LIKELY(last_token == token_type::end_array)) + { + if (JSON_UNLIKELY(not sax->end_array())) + { + return false; + } + + // We are done with this array. Before we can parse a + // new value, we need to evaluate the new state first. + // By setting skip_to_state_evaluation to false, we + // are effectively jumping to the beginning of this if. + assert(not states.empty()); + states.pop_back(); + skip_to_state_evaluation = true; + continue; + } + else + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::end_array, "array"))); + } + } + else // object + { + // comma -> next value + if (get_token() == token_type::value_separator) + { + // parse key + if (JSON_UNLIKELY(get_token() != token_type::value_string)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::value_string, "object key"))); + } + else + { + if (JSON_UNLIKELY(not sax->key(m_lexer.get_string()))) + { + return false; + } + } + + // parse separator (:) + if (JSON_UNLIKELY(get_token() != token_type::name_separator)) + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::name_separator, "object separator"))); + } + + // parse values + get_token(); + continue; + } + + // closing } + if (JSON_LIKELY(last_token == token_type::end_object)) + { + if (JSON_UNLIKELY(not sax->end_object())) + { + return false; + } + + // We are done with this object. Before we can parse a + // new value, we need to evaluate the new state first. + // By setting skip_to_state_evaluation to false, we + // are effectively jumping to the beginning of this if. + assert(not states.empty()); + states.pop_back(); + skip_to_state_evaluation = true; + continue; + } + else + { + return sax->parse_error(m_lexer.get_position(), + m_lexer.get_token_string(), + parse_error::create(101, m_lexer.get_position(), + exception_message(token_type::end_object, "object"))); + } + } + } + } + } + + /// get next token from lexer + token_type get_token() + { + return (last_token = m_lexer.scan()); + } + + std::string exception_message(const token_type expected, const std::string& context) + { + std::string error_msg = "syntax error "; + + if (not context.empty()) + { + error_msg += "while parsing " + context + " "; + } + + error_msg += "- "; + + if (last_token == token_type::parse_error) + { + error_msg += std::string(m_lexer.get_error_message()) + "; last read: '" + + m_lexer.get_token_string() + "'"; + } + else + { + error_msg += "unexpected " + std::string(lexer_t::token_type_name(last_token)); + } + + if (expected != token_type::uninitialized) + { + error_msg += "; expected " + std::string(lexer_t::token_type_name(expected)); + } + + return error_msg; + } + + private: + /// callback function + const parser_callback_t callback = nullptr; + /// the type of the last read token + token_type last_token = token_type::uninitialized; + /// the lexer + lexer_t m_lexer; + /// whether to throw exceptions in case of errors + const bool allow_exceptions = true; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/iterators/primitive_iterator.hpp> + + +#include <cstddef> // ptrdiff_t +#include <limits> // numeric_limits + +namespace nlohmann +{ +namespace detail +{ +/* +@brief an iterator for primitive JSON types + +This class models an iterator for primitive JSON types (boolean, number, +string). It's only purpose is to allow the iterator/const_iterator classes +to "iterate" over primitive values. Internally, the iterator is modeled by +a `difference_type` variable. Value begin_value (`0`) models the begin, +end_value (`1`) models past the end. +*/ +class primitive_iterator_t +{ + private: + using difference_type = std::ptrdiff_t; + static constexpr difference_type begin_value = 0; + static constexpr difference_type end_value = begin_value + 1; + + /// iterator as signed integer type + difference_type m_it = (std::numeric_limits<std::ptrdiff_t>::min)(); + + public: + constexpr difference_type get_value() const noexcept + { + return m_it; + } + + /// set iterator to a defined beginning + void set_begin() noexcept + { + m_it = begin_value; + } + + /// set iterator to a defined past the end + void set_end() noexcept + { + m_it = end_value; + } + + /// return whether the iterator can be dereferenced + constexpr bool is_begin() const noexcept + { + return m_it == begin_value; + } + + /// return whether the iterator is at end + constexpr bool is_end() const noexcept + { + return m_it == end_value; + } + + friend constexpr bool operator==(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept + { + return lhs.m_it == rhs.m_it; + } + + friend constexpr bool operator<(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept + { + return lhs.m_it < rhs.m_it; + } + + primitive_iterator_t operator+(difference_type n) noexcept + { + auto result = *this; + result += n; + return result; + } + + friend constexpr difference_type operator-(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept + { + return lhs.m_it - rhs.m_it; + } + + primitive_iterator_t& operator++() noexcept + { + ++m_it; + return *this; + } + + primitive_iterator_t const operator++(int) noexcept + { + auto result = *this; + ++m_it; + return result; + } + + primitive_iterator_t& operator--() noexcept + { + --m_it; + return *this; + } + + primitive_iterator_t const operator--(int) noexcept + { + auto result = *this; + --m_it; + return result; + } + + primitive_iterator_t& operator+=(difference_type n) noexcept + { + m_it += n; + return *this; + } + + primitive_iterator_t& operator-=(difference_type n) noexcept + { + m_it -= n; + return *this; + } +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/iterators/internal_iterator.hpp> + + +// #include <nlohmann/detail/iterators/primitive_iterator.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/*! +@brief an iterator value + +@note This structure could easily be a union, but MSVC currently does not allow +unions members with complex constructors, see https://github.com/nlohmann/json/pull/105. +*/ +template<typename BasicJsonType> struct internal_iterator +{ + /// iterator for JSON objects + typename BasicJsonType::object_t::iterator object_iterator {}; + /// iterator for JSON arrays + typename BasicJsonType::array_t::iterator array_iterator {}; + /// generic iterator for all other types + primitive_iterator_t primitive_iterator {}; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/iterators/iter_impl.hpp> + + +#include <ciso646> // not +#include <iterator> // iterator, random_access_iterator_tag, bidirectional_iterator_tag, advance, next +#include <type_traits> // conditional, is_const, remove_const + +// #include <nlohmann/detail/exceptions.hpp> + +// #include <nlohmann/detail/iterators/internal_iterator.hpp> + +// #include <nlohmann/detail/iterators/primitive_iterator.hpp> + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/meta/cpp_future.hpp> + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +// forward declare, to be able to friend it later on +template<typename IteratorType> class iteration_proxy; + +/*! +@brief a template for a bidirectional iterator for the @ref basic_json class + +This class implements a both iterators (iterator and const_iterator) for the +@ref basic_json class. + +@note An iterator is called *initialized* when a pointer to a JSON value has + been set (e.g., by a constructor or a copy assignment). If the iterator is + default-constructed, it is *uninitialized* and most methods are undefined. + **The library uses assertions to detect calls on uninitialized iterators.** + +@requirement The class satisfies the following concept requirements: +- +[BidirectionalIterator](https://en.cppreference.com/w/cpp/named_req/BidirectionalIterator): + The iterator that can be moved can be moved in both directions (i.e. + incremented and decremented). + +@since version 1.0.0, simplified in version 2.0.9, change to bidirectional + iterators in version 3.0.0 (see https://github.com/nlohmann/json/issues/593) +*/ +template<typename BasicJsonType> +class iter_impl +{ + /// allow basic_json to access private members + friend iter_impl<typename std::conditional<std::is_const<BasicJsonType>::value, typename std::remove_const<BasicJsonType>::type, const BasicJsonType>::type>; + friend BasicJsonType; + friend iteration_proxy<iter_impl>; + + using object_t = typename BasicJsonType::object_t; + using array_t = typename BasicJsonType::array_t; + // make sure BasicJsonType is basic_json or const basic_json + static_assert(is_basic_json<typename std::remove_const<BasicJsonType>::type>::value, + "iter_impl only accepts (const) basic_json"); + + public: + + /// The std::iterator class template (used as a base class to provide typedefs) is deprecated in C++17. + /// The C++ Standard has never required user-defined iterators to derive from std::iterator. + /// A user-defined iterator should provide publicly accessible typedefs named + /// iterator_category, value_type, difference_type, pointer, and reference. + /// Note that value_type is required to be non-const, even for constant iterators. + using iterator_category = std::bidirectional_iterator_tag; + + /// the type of the values when the iterator is dereferenced + using value_type = typename BasicJsonType::value_type; + /// a type to represent differences between iterators + using difference_type = typename BasicJsonType::difference_type; + /// defines a pointer to the type iterated over (value_type) + using pointer = typename std::conditional<std::is_const<BasicJsonType>::value, + typename BasicJsonType::const_pointer, + typename BasicJsonType::pointer>::type; + /// defines a reference to the type iterated over (value_type) + using reference = + typename std::conditional<std::is_const<BasicJsonType>::value, + typename BasicJsonType::const_reference, + typename BasicJsonType::reference>::type; + + /// default constructor + iter_impl() = default; + + /*! + @brief constructor for a given JSON instance + @param[in] object pointer to a JSON object for this iterator + @pre object != nullptr + @post The iterator is initialized; i.e. `m_object != nullptr`. + */ + explicit iter_impl(pointer object) noexcept : m_object(object) + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + m_it.object_iterator = typename object_t::iterator(); + break; + } + + case value_t::array: + { + m_it.array_iterator = typename array_t::iterator(); + break; + } + + default: + { + m_it.primitive_iterator = primitive_iterator_t(); + break; + } + } + } + + /*! + @note The conventional copy constructor and copy assignment are implicitly + defined. Combined with the following converting constructor and + assignment, they support: (1) copy from iterator to iterator, (2) + copy from const iterator to const iterator, and (3) conversion from + iterator to const iterator. However conversion from const iterator + to iterator is not defined. + */ + + /*! + @brief converting constructor + @param[in] other non-const iterator to copy from + @note It is not checked whether @a other is initialized. + */ + iter_impl(const iter_impl<typename std::remove_const<BasicJsonType>::type>& other) noexcept + : m_object(other.m_object), m_it(other.m_it) {} + + /*! + @brief converting assignment + @param[in,out] other non-const iterator to copy from + @return const/non-const iterator + @note It is not checked whether @a other is initialized. + */ + iter_impl& operator=(const iter_impl<typename std::remove_const<BasicJsonType>::type>& other) noexcept + { + m_object = other.m_object; + m_it = other.m_it; + return *this; + } + + private: + /*! + @brief set the iterator to the first value + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + void set_begin() noexcept + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + m_it.object_iterator = m_object->m_value.object->begin(); + break; + } + + case value_t::array: + { + m_it.array_iterator = m_object->m_value.array->begin(); + break; + } + + case value_t::null: + { + // set to end so begin()==end() is true: null is empty + m_it.primitive_iterator.set_end(); + break; + } + + default: + { + m_it.primitive_iterator.set_begin(); + break; + } + } + } + + /*! + @brief set the iterator past the last value + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + void set_end() noexcept + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + m_it.object_iterator = m_object->m_value.object->end(); + break; + } + + case value_t::array: + { + m_it.array_iterator = m_object->m_value.array->end(); + break; + } + + default: + { + m_it.primitive_iterator.set_end(); + break; + } + } + } + + public: + /*! + @brief return a reference to the value pointed to by the iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + reference operator*() const + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + assert(m_it.object_iterator != m_object->m_value.object->end()); + return m_it.object_iterator->second; + } + + case value_t::array: + { + assert(m_it.array_iterator != m_object->m_value.array->end()); + return *m_it.array_iterator; + } + + case value_t::null: + JSON_THROW(invalid_iterator::create(214, "cannot get value")); + + default: + { + if (JSON_LIKELY(m_it.primitive_iterator.is_begin())) + { + return *m_object; + } + + JSON_THROW(invalid_iterator::create(214, "cannot get value")); + } + } + } + + /*! + @brief dereference the iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + pointer operator->() const + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + assert(m_it.object_iterator != m_object->m_value.object->end()); + return &(m_it.object_iterator->second); + } + + case value_t::array: + { + assert(m_it.array_iterator != m_object->m_value.array->end()); + return &*m_it.array_iterator; + } + + default: + { + if (JSON_LIKELY(m_it.primitive_iterator.is_begin())) + { + return m_object; + } + + JSON_THROW(invalid_iterator::create(214, "cannot get value")); + } + } + } + + /*! + @brief post-increment (it++) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl const operator++(int) + { + auto result = *this; + ++(*this); + return result; + } + + /*! + @brief pre-increment (++it) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator++() + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + std::advance(m_it.object_iterator, 1); + break; + } + + case value_t::array: + { + std::advance(m_it.array_iterator, 1); + break; + } + + default: + { + ++m_it.primitive_iterator; + break; + } + } + + return *this; + } + + /*! + @brief post-decrement (it--) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl const operator--(int) + { + auto result = *this; + --(*this); + return result; + } + + /*! + @brief pre-decrement (--it) + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator--() + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + { + std::advance(m_it.object_iterator, -1); + break; + } + + case value_t::array: + { + std::advance(m_it.array_iterator, -1); + break; + } + + default: + { + --m_it.primitive_iterator; + break; + } + } + + return *this; + } + + /*! + @brief comparison: equal + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + bool operator==(const iter_impl& other) const + { + // if objects are not the same, the comparison is undefined + if (JSON_UNLIKELY(m_object != other.m_object)) + { + JSON_THROW(invalid_iterator::create(212, "cannot compare iterators of different containers")); + } + + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + return (m_it.object_iterator == other.m_it.object_iterator); + + case value_t::array: + return (m_it.array_iterator == other.m_it.array_iterator); + + default: + return (m_it.primitive_iterator == other.m_it.primitive_iterator); + } + } + + /*! + @brief comparison: not equal + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + bool operator!=(const iter_impl& other) const + { + return not operator==(other); + } + + /*! + @brief comparison: smaller + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + bool operator<(const iter_impl& other) const + { + // if objects are not the same, the comparison is undefined + if (JSON_UNLIKELY(m_object != other.m_object)) + { + JSON_THROW(invalid_iterator::create(212, "cannot compare iterators of different containers")); + } + + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(213, "cannot compare order of object iterators")); + + case value_t::array: + return (m_it.array_iterator < other.m_it.array_iterator); + + default: + return (m_it.primitive_iterator < other.m_it.primitive_iterator); + } + } + + /*! + @brief comparison: less than or equal + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + bool operator<=(const iter_impl& other) const + { + return not other.operator < (*this); + } + + /*! + @brief comparison: greater than + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + bool operator>(const iter_impl& other) const + { + return not operator<=(other); + } + + /*! + @brief comparison: greater than or equal + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + bool operator>=(const iter_impl& other) const + { + return not operator<(other); + } + + /*! + @brief add to iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator+=(difference_type i) + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(209, "cannot use offsets with object iterators")); + + case value_t::array: + { + std::advance(m_it.array_iterator, i); + break; + } + + default: + { + m_it.primitive_iterator += i; + break; + } + } + + return *this; + } + + /*! + @brief subtract from iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl& operator-=(difference_type i) + { + return operator+=(-i); + } + + /*! + @brief add to iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl operator+(difference_type i) const + { + auto result = *this; + result += i; + return result; + } + + /*! + @brief addition of distance and iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + friend iter_impl operator+(difference_type i, const iter_impl& it) + { + auto result = it; + result += i; + return result; + } + + /*! + @brief subtract from iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + iter_impl operator-(difference_type i) const + { + auto result = *this; + result -= i; + return result; + } + + /*! + @brief return difference + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + difference_type operator-(const iter_impl& other) const + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(209, "cannot use offsets with object iterators")); + + case value_t::array: + return m_it.array_iterator - other.m_it.array_iterator; + + default: + return m_it.primitive_iterator - other.m_it.primitive_iterator; + } + } + + /*! + @brief access to successor + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + reference operator[](difference_type n) const + { + assert(m_object != nullptr); + + switch (m_object->m_type) + { + case value_t::object: + JSON_THROW(invalid_iterator::create(208, "cannot use operator[] for object iterators")); + + case value_t::array: + return *std::next(m_it.array_iterator, n); + + case value_t::null: + JSON_THROW(invalid_iterator::create(214, "cannot get value")); + + default: + { + if (JSON_LIKELY(m_it.primitive_iterator.get_value() == -n)) + { + return *m_object; + } + + JSON_THROW(invalid_iterator::create(214, "cannot get value")); + } + } + } + + /*! + @brief return the key of an object iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + const typename object_t::key_type& key() const + { + assert(m_object != nullptr); + + if (JSON_LIKELY(m_object->is_object())) + { + return m_it.object_iterator->first; + } + + JSON_THROW(invalid_iterator::create(207, "cannot use key() for non-object iterators")); + } + + /*! + @brief return the value of an iterator + @pre The iterator is initialized; i.e. `m_object != nullptr`. + */ + reference value() const + { + return operator*(); + } + + private: + /// associated JSON instance + pointer m_object = nullptr; + /// the actual iterator of the associated instance + internal_iterator<typename std::remove_const<BasicJsonType>::type> m_it; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/iterators/iteration_proxy.hpp> + +// #include <nlohmann/detail/iterators/json_reverse_iterator.hpp> + + +#include <cstddef> // ptrdiff_t +#include <iterator> // reverse_iterator +#include <utility> // declval + +namespace nlohmann +{ +namespace detail +{ +////////////////////// +// reverse_iterator // +////////////////////// + +/*! +@brief a template for a reverse iterator class + +@tparam Base the base iterator type to reverse. Valid types are @ref +iterator (to create @ref reverse_iterator) and @ref const_iterator (to +create @ref const_reverse_iterator). + +@requirement The class satisfies the following concept requirements: +- +[BidirectionalIterator](https://en.cppreference.com/w/cpp/named_req/BidirectionalIterator): + The iterator that can be moved can be moved in both directions (i.e. + incremented and decremented). +- [OutputIterator](https://en.cppreference.com/w/cpp/named_req/OutputIterator): + It is possible to write to the pointed-to element (only if @a Base is + @ref iterator). + +@since version 1.0.0 +*/ +template<typename Base> +class json_reverse_iterator : public std::reverse_iterator<Base> +{ + public: + using difference_type = std::ptrdiff_t; + /// shortcut to the reverse iterator adapter + using base_iterator = std::reverse_iterator<Base>; + /// the reference type for the pointed-to element + using reference = typename Base::reference; + + /// create reverse iterator from iterator + explicit json_reverse_iterator(const typename base_iterator::iterator_type& it) noexcept + : base_iterator(it) {} + + /// create reverse iterator from base class + explicit json_reverse_iterator(const base_iterator& it) noexcept : base_iterator(it) {} + + /// post-increment (it++) + json_reverse_iterator const operator++(int) + { + return static_cast<json_reverse_iterator>(base_iterator::operator++(1)); + } + + /// pre-increment (++it) + json_reverse_iterator& operator++() + { + return static_cast<json_reverse_iterator&>(base_iterator::operator++()); + } + + /// post-decrement (it--) + json_reverse_iterator const operator--(int) + { + return static_cast<json_reverse_iterator>(base_iterator::operator--(1)); + } + + /// pre-decrement (--it) + json_reverse_iterator& operator--() + { + return static_cast<json_reverse_iterator&>(base_iterator::operator--()); + } + + /// add to iterator + json_reverse_iterator& operator+=(difference_type i) + { + return static_cast<json_reverse_iterator&>(base_iterator::operator+=(i)); + } + + /// add to iterator + json_reverse_iterator operator+(difference_type i) const + { + return static_cast<json_reverse_iterator>(base_iterator::operator+(i)); + } + + /// subtract from iterator + json_reverse_iterator operator-(difference_type i) const + { + return static_cast<json_reverse_iterator>(base_iterator::operator-(i)); + } + + /// return difference + difference_type operator-(const json_reverse_iterator& other) const + { + return base_iterator(*this) - base_iterator(other); + } + + /// access to successor + reference operator[](difference_type n) const + { + return *(this->operator+(n)); + } + + /// return the key of an object iterator + auto key() const -> decltype(std::declval<Base>().key()) + { + auto it = --this->base(); + return it.key(); + } + + /// return the value of an iterator + reference value() const + { + auto it = --this->base(); + return it.operator * (); + } +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/output/output_adapters.hpp> + + +#include <algorithm> // copy +#include <cstddef> // size_t +#include <ios> // streamsize +#include <iterator> // back_inserter +#include <memory> // shared_ptr, make_shared +#include <ostream> // basic_ostream +#include <string> // basic_string +#include <vector> // vector + +namespace nlohmann +{ +namespace detail +{ +/// abstract output adapter interface +template<typename CharType> struct output_adapter_protocol +{ + virtual void write_character(CharType c) = 0; + virtual void write_characters(const CharType* s, std::size_t length) = 0; + virtual ~output_adapter_protocol() = default; +}; + +/// a type to simplify interfaces +template<typename CharType> +using output_adapter_t = std::shared_ptr<output_adapter_protocol<CharType>>; + +/// output adapter for byte vectors +template<typename CharType> +class output_vector_adapter : public output_adapter_protocol<CharType> +{ + public: + explicit output_vector_adapter(std::vector<CharType>& vec) noexcept + : v(vec) + {} + + void write_character(CharType c) override + { + v.push_back(c); + } + + void write_characters(const CharType* s, std::size_t length) override + { + std::copy(s, s + length, std::back_inserter(v)); + } + + private: + std::vector<CharType>& v; +}; + +/// output adapter for output streams +template<typename CharType> +class output_stream_adapter : public output_adapter_protocol<CharType> +{ + public: + explicit output_stream_adapter(std::basic_ostream<CharType>& s) noexcept + : stream(s) + {} + + void write_character(CharType c) override + { + stream.put(c); + } + + void write_characters(const CharType* s, std::size_t length) override + { + stream.write(s, static_cast<std::streamsize>(length)); + } + + private: + std::basic_ostream<CharType>& stream; +}; + +/// output adapter for basic_string +template<typename CharType, typename StringType = std::basic_string<CharType>> +class output_string_adapter : public output_adapter_protocol<CharType> +{ + public: + explicit output_string_adapter(StringType& s) noexcept + : str(s) + {} + + void write_character(CharType c) override + { + str.push_back(c); + } + + void write_characters(const CharType* s, std::size_t length) override + { + str.append(s, length); + } + + private: + StringType& str; +}; + +template<typename CharType, typename StringType = std::basic_string<CharType>> +class output_adapter +{ + public: + output_adapter(std::vector<CharType>& vec) + : oa(std::make_shared<output_vector_adapter<CharType>>(vec)) {} + + output_adapter(std::basic_ostream<CharType>& s) + : oa(std::make_shared<output_stream_adapter<CharType>>(s)) {} + + output_adapter(StringType& s) + : oa(std::make_shared<output_string_adapter<CharType, StringType>>(s)) {} + + operator output_adapter_t<CharType>() + { + return oa; + } + + private: + output_adapter_t<CharType> oa = nullptr; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/input/binary_reader.hpp> + + +#include <algorithm> // generate_n +#include <array> // array +#include <cassert> // assert +#include <cmath> // ldexp +#include <cstddef> // size_t +#include <cstdint> // uint8_t, uint16_t, uint32_t, uint64_t +#include <cstdio> // snprintf +#include <cstring> // memcpy +#include <iterator> // back_inserter +#include <limits> // numeric_limits +#include <string> // char_traits, string +#include <utility> // make_pair, move + +// #include <nlohmann/detail/input/input_adapters.hpp> + +// #include <nlohmann/detail/input/json_sax.hpp> + +// #include <nlohmann/detail/exceptions.hpp> + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/meta/is_sax.hpp> + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/////////////////// +// binary reader // +/////////////////// + +/*! +@brief deserialization of CBOR, MessagePack, and UBJSON values +*/ +template<typename BasicJsonType, typename SAX = json_sax_dom_parser<BasicJsonType>> +class binary_reader +{ + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using number_float_t = typename BasicJsonType::number_float_t; + using string_t = typename BasicJsonType::string_t; + using json_sax_t = SAX; + + public: + /*! + @brief create a binary reader + + @param[in] adapter input adapter to read from + */ + explicit binary_reader(input_adapter_t adapter) : ia(std::move(adapter)) + { + (void)detail::is_sax_static_asserts<SAX, BasicJsonType> {}; + assert(ia); + } + + /*! + @param[in] format the binary format to parse + @param[in] sax_ a SAX event processor + @param[in] strict whether to expect the input to be consumed completed + + @return + */ + bool sax_parse(const input_format_t format, + json_sax_t* sax_, + const bool strict = true) + { + sax = sax_; + bool result = false; + + switch (format) + { + case input_format_t::bson: + result = parse_bson_internal(); + break; + + case input_format_t::cbor: + result = parse_cbor_internal(); + break; + + case input_format_t::msgpack: + result = parse_msgpack_internal(); + break; + + case input_format_t::ubjson: + result = parse_ubjson_internal(); + break; + + // LCOV_EXCL_START + default: + assert(false); + // LCOV_EXCL_STOP + } + + // strict mode: next byte must be EOF + if (result and strict) + { + if (format == input_format_t::ubjson) + { + get_ignore_noop(); + } + else + { + get(); + } + + if (JSON_UNLIKELY(current != std::char_traits<char>::eof())) + { + return sax->parse_error(chars_read, get_token_string(), + parse_error::create(110, chars_read, exception_message(format, "expected end of input; last byte: 0x" + get_token_string(), "value"))); + } + } + + return result; + } + + /*! + @brief determine system byte order + + @return true if and only if system's byte order is little endian + + @note from http://stackoverflow.com/a/1001328/266378 + */ + static constexpr bool little_endianess(int num = 1) noexcept + { + return (*reinterpret_cast<char*>(&num) == 1); + } + + private: + ////////// + // BSON // + ////////// + + /*! + @brief Reads in a BSON-object and passes it to the SAX-parser. + @return whether a valid BSON-value was passed to the SAX parser + */ + bool parse_bson_internal() + { + std::int32_t document_size; + get_number<std::int32_t, true>(input_format_t::bson, document_size); + + if (JSON_UNLIKELY(not sax->start_object(std::size_t(-1)))) + { + return false; + } + + if (JSON_UNLIKELY(not parse_bson_element_list(/*is_array*/false))) + { + return false; + } + + return sax->end_object(); + } + + /*! + @brief Parses a C-style string from the BSON input. + @param[in, out] result A reference to the string variable where the read + string is to be stored. + @return `true` if the \x00-byte indicating the end of the string was + encountered before the EOF; false` indicates an unexpected EOF. + */ + bool get_bson_cstr(string_t& result) + { + auto out = std::back_inserter(result); + while (true) + { + get(); + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::bson, "cstring"))) + { + return false; + } + if (current == 0x00) + { + return true; + } + *out++ = static_cast<char>(current); + } + + return true; + } + + /*! + @brief Parses a zero-terminated string of length @a len from the BSON + input. + @param[in] len The length (including the zero-byte at the end) of the + string to be read. + @param[in, out] result A reference to the string variable where the read + string is to be stored. + @tparam NumberType The type of the length @a len + @pre len >= 1 + @return `true` if the string was successfully parsed + */ + template<typename NumberType> + bool get_bson_string(const NumberType len, string_t& result) + { + if (JSON_UNLIKELY(len < 1)) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format_t::bson, "string length must be at least 1, is " + std::to_string(len), "string"))); + } + + return get_string(input_format_t::bson, len - static_cast<NumberType>(1), result) and get() != std::char_traits<char>::eof(); + } + + /*! + @brief Read a BSON document element of the given @a element_type. + @param[in] element_type The BSON element type, c.f. http://bsonspec.org/spec.html + @param[in] element_type_parse_position The position in the input stream, + where the `element_type` was read. + @warning Not all BSON element types are supported yet. An unsupported + @a element_type will give rise to a parse_error.114: + Unsupported BSON record type 0x... + @return whether a valid BSON-object/array was passed to the SAX parser + */ + bool parse_bson_element_internal(const int element_type, + const std::size_t element_type_parse_position) + { + switch (element_type) + { + case 0x01: // double + { + double number; + return get_number<double, true>(input_format_t::bson, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + case 0x02: // string + { + std::int32_t len; + string_t value; + return get_number<std::int32_t, true>(input_format_t::bson, len) and get_bson_string(len, value) and sax->string(value); + } + + case 0x03: // object + { + return parse_bson_internal(); + } + + case 0x04: // array + { + return parse_bson_array(); + } + + case 0x08: // boolean + { + return sax->boolean(static_cast<bool>(get())); + } + + case 0x0A: // null + { + return sax->null(); + } + + case 0x10: // int32 + { + std::int32_t value; + return get_number<std::int32_t, true>(input_format_t::bson, value) and sax->number_integer(value); + } + + case 0x12: // int64 + { + std::int64_t value; + return get_number<std::int64_t, true>(input_format_t::bson, value) and sax->number_integer(value); + } + + default: // anything else not supported (yet) + { + char cr[3]; + snprintf(cr, sizeof(cr), "%.2hhX", static_cast<unsigned char>(element_type)); + return sax->parse_error(element_type_parse_position, std::string(cr), parse_error::create(114, element_type_parse_position, "Unsupported BSON record type 0x" + std::string(cr))); + } + } + } + + /*! + @brief Read a BSON element list (as specified in the BSON-spec) + + The same binary layout is used for objects and arrays, hence it must be + indicated with the argument @a is_array which one is expected + (true --> array, false --> object). + + @param[in] is_array Determines if the element list being read is to be + treated as an object (@a is_array == false), or as an + array (@a is_array == true). + @return whether a valid BSON-object/array was passed to the SAX parser + */ + bool parse_bson_element_list(const bool is_array) + { + string_t key; + while (int element_type = get()) + { + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::bson, "element list"))) + { + return false; + } + + const std::size_t element_type_parse_position = chars_read; + if (JSON_UNLIKELY(not get_bson_cstr(key))) + { + return false; + } + + if (not is_array) + { + if (not sax->key(key)) + { + return false; + } + } + + if (JSON_UNLIKELY(not parse_bson_element_internal(element_type, element_type_parse_position))) + { + return false; + } + + // get_bson_cstr only appends + key.clear(); + } + + return true; + } + + /*! + @brief Reads an array from the BSON input and passes it to the SAX-parser. + @return whether a valid BSON-array was passed to the SAX parser + */ + bool parse_bson_array() + { + std::int32_t document_size; + get_number<std::int32_t, true>(input_format_t::bson, document_size); + + if (JSON_UNLIKELY(not sax->start_array(std::size_t(-1)))) + { + return false; + } + + if (JSON_UNLIKELY(not parse_bson_element_list(/*is_array*/true))) + { + return false; + } + + return sax->end_array(); + } + + ////////// + // CBOR // + ////////// + + /*! + @param[in] get_char whether a new character should be retrieved from the + input (true, default) or whether the last read + character should be considered instead + + @return whether a valid CBOR value was passed to the SAX parser + */ + bool parse_cbor_internal(const bool get_char = true) + { + switch (get_char ? get() : current) + { + // EOF + case std::char_traits<char>::eof(): + return unexpect_eof(input_format_t::cbor, "value"); + + // Integer 0x00..0x17 (0..23) + case 0x00: + case 0x01: + case 0x02: + case 0x03: + case 0x04: + case 0x05: + case 0x06: + case 0x07: + case 0x08: + case 0x09: + case 0x0A: + case 0x0B: + case 0x0C: + case 0x0D: + case 0x0E: + case 0x0F: + case 0x10: + case 0x11: + case 0x12: + case 0x13: + case 0x14: + case 0x15: + case 0x16: + case 0x17: + return sax->number_unsigned(static_cast<number_unsigned_t>(current)); + + case 0x18: // Unsigned integer (one-byte uint8_t follows) + { + uint8_t number; + return get_number(input_format_t::cbor, number) and sax->number_unsigned(number); + } + + case 0x19: // Unsigned integer (two-byte uint16_t follows) + { + uint16_t number; + return get_number(input_format_t::cbor, number) and sax->number_unsigned(number); + } + + case 0x1A: // Unsigned integer (four-byte uint32_t follows) + { + uint32_t number; + return get_number(input_format_t::cbor, number) and sax->number_unsigned(number); + } + + case 0x1B: // Unsigned integer (eight-byte uint64_t follows) + { + uint64_t number; + return get_number(input_format_t::cbor, number) and sax->number_unsigned(number); + } + + // Negative integer -1-0x00..-1-0x17 (-1..-24) + case 0x20: + case 0x21: + case 0x22: + case 0x23: + case 0x24: + case 0x25: + case 0x26: + case 0x27: + case 0x28: + case 0x29: + case 0x2A: + case 0x2B: + case 0x2C: + case 0x2D: + case 0x2E: + case 0x2F: + case 0x30: + case 0x31: + case 0x32: + case 0x33: + case 0x34: + case 0x35: + case 0x36: + case 0x37: + return sax->number_integer(static_cast<int8_t>(0x20 - 1 - current)); + + case 0x38: // Negative integer (one-byte uint8_t follows) + { + uint8_t number; + return get_number(input_format_t::cbor, number) and sax->number_integer(static_cast<number_integer_t>(-1) - number); + } + + case 0x39: // Negative integer -1-n (two-byte uint16_t follows) + { + uint16_t number; + return get_number(input_format_t::cbor, number) and sax->number_integer(static_cast<number_integer_t>(-1) - number); + } + + case 0x3A: // Negative integer -1-n (four-byte uint32_t follows) + { + uint32_t number; + return get_number(input_format_t::cbor, number) and sax->number_integer(static_cast<number_integer_t>(-1) - number); + } + + case 0x3B: // Negative integer -1-n (eight-byte uint64_t follows) + { + uint64_t number; + return get_number(input_format_t::cbor, number) and sax->number_integer(static_cast<number_integer_t>(-1) + - static_cast<number_integer_t>(number)); + } + + // UTF-8 string (0x00..0x17 bytes follow) + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + case 0x78: // UTF-8 string (one-byte uint8_t for n follows) + case 0x79: // UTF-8 string (two-byte uint16_t for n follow) + case 0x7A: // UTF-8 string (four-byte uint32_t for n follow) + case 0x7B: // UTF-8 string (eight-byte uint64_t for n follow) + case 0x7F: // UTF-8 string (indefinite length) + { + string_t s; + return get_cbor_string(s) and sax->string(s); + } + + // array (0x00..0x17 data items follow) + case 0x80: + case 0x81: + case 0x82: + case 0x83: + case 0x84: + case 0x85: + case 0x86: + case 0x87: + case 0x88: + case 0x89: + case 0x8A: + case 0x8B: + case 0x8C: + case 0x8D: + case 0x8E: + case 0x8F: + case 0x90: + case 0x91: + case 0x92: + case 0x93: + case 0x94: + case 0x95: + case 0x96: + case 0x97: + return get_cbor_array(static_cast<std::size_t>(current & 0x1F)); + + case 0x98: // array (one-byte uint8_t for n follows) + { + uint8_t len; + return get_number(input_format_t::cbor, len) and get_cbor_array(static_cast<std::size_t>(len)); + } + + case 0x99: // array (two-byte uint16_t for n follow) + { + uint16_t len; + return get_number(input_format_t::cbor, len) and get_cbor_array(static_cast<std::size_t>(len)); + } + + case 0x9A: // array (four-byte uint32_t for n follow) + { + uint32_t len; + return get_number(input_format_t::cbor, len) and get_cbor_array(static_cast<std::size_t>(len)); + } + + case 0x9B: // array (eight-byte uint64_t for n follow) + { + uint64_t len; + return get_number(input_format_t::cbor, len) and get_cbor_array(static_cast<std::size_t>(len)); + } + + case 0x9F: // array (indefinite length) + return get_cbor_array(std::size_t(-1)); + + // map (0x00..0x17 pairs of data items follow) + case 0xA0: + case 0xA1: + case 0xA2: + case 0xA3: + case 0xA4: + case 0xA5: + case 0xA6: + case 0xA7: + case 0xA8: + case 0xA9: + case 0xAA: + case 0xAB: + case 0xAC: + case 0xAD: + case 0xAE: + case 0xAF: + case 0xB0: + case 0xB1: + case 0xB2: + case 0xB3: + case 0xB4: + case 0xB5: + case 0xB6: + case 0xB7: + return get_cbor_object(static_cast<std::size_t>(current & 0x1F)); + + case 0xB8: // map (one-byte uint8_t for n follows) + { + uint8_t len; + return get_number(input_format_t::cbor, len) and get_cbor_object(static_cast<std::size_t>(len)); + } + + case 0xB9: // map (two-byte uint16_t for n follow) + { + uint16_t len; + return get_number(input_format_t::cbor, len) and get_cbor_object(static_cast<std::size_t>(len)); + } + + case 0xBA: // map (four-byte uint32_t for n follow) + { + uint32_t len; + return get_number(input_format_t::cbor, len) and get_cbor_object(static_cast<std::size_t>(len)); + } + + case 0xBB: // map (eight-byte uint64_t for n follow) + { + uint64_t len; + return get_number(input_format_t::cbor, len) and get_cbor_object(static_cast<std::size_t>(len)); + } + + case 0xBF: // map (indefinite length) + return get_cbor_object(std::size_t(-1)); + + case 0xF4: // false + return sax->boolean(false); + + case 0xF5: // true + return sax->boolean(true); + + case 0xF6: // null + return sax->null(); + + case 0xF9: // Half-Precision Float (two-byte IEEE 754) + { + const int byte1_raw = get(); + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::cbor, "number"))) + { + return false; + } + const int byte2_raw = get(); + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::cbor, "number"))) + { + return false; + } + + const auto byte1 = static_cast<unsigned char>(byte1_raw); + const auto byte2 = static_cast<unsigned char>(byte2_raw); + + // code from RFC 7049, Appendix D, Figure 3: + // As half-precision floating-point numbers were only added + // to IEEE 754 in 2008, today's programming platforms often + // still only have limited support for them. It is very + // easy to include at least decoding support for them even + // without such support. An example of a small decoder for + // half-precision floating-point numbers in the C language + // is shown in Fig. 3. + const int half = (byte1 << 8) + byte2; + const double val = [&half] + { + const int exp = (half >> 10) & 0x1F; + const int mant = half & 0x3FF; + assert(0 <= exp and exp <= 32); + assert(0 <= mant and mant <= 1024); + switch (exp) + { + case 0: + return std::ldexp(mant, -24); + case 31: + return (mant == 0) + ? std::numeric_limits<double>::infinity() + : std::numeric_limits<double>::quiet_NaN(); + default: + return std::ldexp(mant + 1024, exp - 25); + } + }(); + return sax->number_float((half & 0x8000) != 0 + ? static_cast<number_float_t>(-val) + : static_cast<number_float_t>(val), ""); + } + + case 0xFA: // Single-Precision Float (four-byte IEEE 754) + { + float number; + return get_number(input_format_t::cbor, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + case 0xFB: // Double-Precision Float (eight-byte IEEE 754) + { + double number; + return get_number(input_format_t::cbor, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + default: // anything else (0xFF is handled inside the other types) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format_t::cbor, "invalid byte: 0x" + last_token, "value"))); + } + } + } + + /*! + @brief reads a CBOR string + + This function first reads starting bytes to determine the expected + string length and then copies this number of bytes into a string. + Additionally, CBOR's strings with indefinite lengths are supported. + + @param[out] result created string + + @return whether string creation completed + */ + bool get_cbor_string(string_t& result) + { + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::cbor, "string"))) + { + return false; + } + + switch (current) + { + // UTF-8 string (0x00..0x17 bytes follow) + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + { + return get_string(input_format_t::cbor, current & 0x1F, result); + } + + case 0x78: // UTF-8 string (one-byte uint8_t for n follows) + { + uint8_t len; + return get_number(input_format_t::cbor, len) and get_string(input_format_t::cbor, len, result); + } + + case 0x79: // UTF-8 string (two-byte uint16_t for n follow) + { + uint16_t len; + return get_number(input_format_t::cbor, len) and get_string(input_format_t::cbor, len, result); + } + + case 0x7A: // UTF-8 string (four-byte uint32_t for n follow) + { + uint32_t len; + return get_number(input_format_t::cbor, len) and get_string(input_format_t::cbor, len, result); + } + + case 0x7B: // UTF-8 string (eight-byte uint64_t for n follow) + { + uint64_t len; + return get_number(input_format_t::cbor, len) and get_string(input_format_t::cbor, len, result); + } + + case 0x7F: // UTF-8 string (indefinite length) + { + while (get() != 0xFF) + { + string_t chunk; + if (not get_cbor_string(chunk)) + { + return false; + } + result.append(chunk); + } + return true; + } + + default: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format_t::cbor, "expected length specification (0x60-0x7B) or indefinite string type (0x7F); last byte: 0x" + last_token, "string"))); + } + } + } + + /*! + @param[in] len the length of the array or std::size_t(-1) for an + array of indefinite size + @return whether array creation completed + */ + bool get_cbor_array(const std::size_t len) + { + if (JSON_UNLIKELY(not sax->start_array(len))) + { + return false; + } + + if (len != std::size_t(-1)) + { + for (std::size_t i = 0; i < len; ++i) + { + if (JSON_UNLIKELY(not parse_cbor_internal())) + { + return false; + } + } + } + else + { + while (get() != 0xFF) + { + if (JSON_UNLIKELY(not parse_cbor_internal(false))) + { + return false; + } + } + } + + return sax->end_array(); + } + + /*! + @param[in] len the length of the object or std::size_t(-1) for an + object of indefinite size + @return whether object creation completed + */ + bool get_cbor_object(const std::size_t len) + { + if (not JSON_UNLIKELY(sax->start_object(len))) + { + return false; + } + + string_t key; + if (len != std::size_t(-1)) + { + for (std::size_t i = 0; i < len; ++i) + { + get(); + if (JSON_UNLIKELY(not get_cbor_string(key) or not sax->key(key))) + { + return false; + } + + if (JSON_UNLIKELY(not parse_cbor_internal())) + { + return false; + } + key.clear(); + } + } + else + { + while (get() != 0xFF) + { + if (JSON_UNLIKELY(not get_cbor_string(key) or not sax->key(key))) + { + return false; + } + + if (JSON_UNLIKELY(not parse_cbor_internal())) + { + return false; + } + key.clear(); + } + } + + return sax->end_object(); + } + + ///////////// + // MsgPack // + ///////////// + + /*! + @return whether a valid MessagePack value was passed to the SAX parser + */ + bool parse_msgpack_internal() + { + switch (get()) + { + // EOF + case std::char_traits<char>::eof(): + return unexpect_eof(input_format_t::msgpack, "value"); + + // positive fixint + case 0x00: + case 0x01: + case 0x02: + case 0x03: + case 0x04: + case 0x05: + case 0x06: + case 0x07: + case 0x08: + case 0x09: + case 0x0A: + case 0x0B: + case 0x0C: + case 0x0D: + case 0x0E: + case 0x0F: + case 0x10: + case 0x11: + case 0x12: + case 0x13: + case 0x14: + case 0x15: + case 0x16: + case 0x17: + case 0x18: + case 0x19: + case 0x1A: + case 0x1B: + case 0x1C: + case 0x1D: + case 0x1E: + case 0x1F: + case 0x20: + case 0x21: + case 0x22: + case 0x23: + case 0x24: + case 0x25: + case 0x26: + case 0x27: + case 0x28: + case 0x29: + case 0x2A: + case 0x2B: + case 0x2C: + case 0x2D: + case 0x2E: + case 0x2F: + case 0x30: + case 0x31: + case 0x32: + case 0x33: + case 0x34: + case 0x35: + case 0x36: + case 0x37: + case 0x38: + case 0x39: + case 0x3A: + case 0x3B: + case 0x3C: + case 0x3D: + case 0x3E: + case 0x3F: + case 0x40: + case 0x41: + case 0x42: + case 0x43: + case 0x44: + case 0x45: + case 0x46: + case 0x47: + case 0x48: + case 0x49: + case 0x4A: + case 0x4B: + case 0x4C: + case 0x4D: + case 0x4E: + case 0x4F: + case 0x50: + case 0x51: + case 0x52: + case 0x53: + case 0x54: + case 0x55: + case 0x56: + case 0x57: + case 0x58: + case 0x59: + case 0x5A: + case 0x5B: + case 0x5C: + case 0x5D: + case 0x5E: + case 0x5F: + case 0x60: + case 0x61: + case 0x62: + case 0x63: + case 0x64: + case 0x65: + case 0x66: + case 0x67: + case 0x68: + case 0x69: + case 0x6A: + case 0x6B: + case 0x6C: + case 0x6D: + case 0x6E: + case 0x6F: + case 0x70: + case 0x71: + case 0x72: + case 0x73: + case 0x74: + case 0x75: + case 0x76: + case 0x77: + case 0x78: + case 0x79: + case 0x7A: + case 0x7B: + case 0x7C: + case 0x7D: + case 0x7E: + case 0x7F: + return sax->number_unsigned(static_cast<number_unsigned_t>(current)); + + // fixmap + case 0x80: + case 0x81: + case 0x82: + case 0x83: + case 0x84: + case 0x85: + case 0x86: + case 0x87: + case 0x88: + case 0x89: + case 0x8A: + case 0x8B: + case 0x8C: + case 0x8D: + case 0x8E: + case 0x8F: + return get_msgpack_object(static_cast<std::size_t>(current & 0x0F)); + + // fixarray + case 0x90: + case 0x91: + case 0x92: + case 0x93: + case 0x94: + case 0x95: + case 0x96: + case 0x97: + case 0x98: + case 0x99: + case 0x9A: + case 0x9B: + case 0x9C: + case 0x9D: + case 0x9E: + case 0x9F: + return get_msgpack_array(static_cast<std::size_t>(current & 0x0F)); + + // fixstr + case 0xA0: + case 0xA1: + case 0xA2: + case 0xA3: + case 0xA4: + case 0xA5: + case 0xA6: + case 0xA7: + case 0xA8: + case 0xA9: + case 0xAA: + case 0xAB: + case 0xAC: + case 0xAD: + case 0xAE: + case 0xAF: + case 0xB0: + case 0xB1: + case 0xB2: + case 0xB3: + case 0xB4: + case 0xB5: + case 0xB6: + case 0xB7: + case 0xB8: + case 0xB9: + case 0xBA: + case 0xBB: + case 0xBC: + case 0xBD: + case 0xBE: + case 0xBF: + { + string_t s; + return get_msgpack_string(s) and sax->string(s); + } + + case 0xC0: // nil + return sax->null(); + + case 0xC2: // false + return sax->boolean(false); + + case 0xC3: // true + return sax->boolean(true); + + case 0xCA: // float 32 + { + float number; + return get_number(input_format_t::msgpack, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + case 0xCB: // float 64 + { + double number; + return get_number(input_format_t::msgpack, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + case 0xCC: // uint 8 + { + uint8_t number; + return get_number(input_format_t::msgpack, number) and sax->number_unsigned(number); + } + + case 0xCD: // uint 16 + { + uint16_t number; + return get_number(input_format_t::msgpack, number) and sax->number_unsigned(number); + } + + case 0xCE: // uint 32 + { + uint32_t number; + return get_number(input_format_t::msgpack, number) and sax->number_unsigned(number); + } + + case 0xCF: // uint 64 + { + uint64_t number; + return get_number(input_format_t::msgpack, number) and sax->number_unsigned(number); + } + + case 0xD0: // int 8 + { + int8_t number; + return get_number(input_format_t::msgpack, number) and sax->number_integer(number); + } + + case 0xD1: // int 16 + { + int16_t number; + return get_number(input_format_t::msgpack, number) and sax->number_integer(number); + } + + case 0xD2: // int 32 + { + int32_t number; + return get_number(input_format_t::msgpack, number) and sax->number_integer(number); + } + + case 0xD3: // int 64 + { + int64_t number; + return get_number(input_format_t::msgpack, number) and sax->number_integer(number); + } + + case 0xD9: // str 8 + case 0xDA: // str 16 + case 0xDB: // str 32 + { + string_t s; + return get_msgpack_string(s) and sax->string(s); + } + + case 0xDC: // array 16 + { + uint16_t len; + return get_number(input_format_t::msgpack, len) and get_msgpack_array(static_cast<std::size_t>(len)); + } + + case 0xDD: // array 32 + { + uint32_t len; + return get_number(input_format_t::msgpack, len) and get_msgpack_array(static_cast<std::size_t>(len)); + } + + case 0xDE: // map 16 + { + uint16_t len; + return get_number(input_format_t::msgpack, len) and get_msgpack_object(static_cast<std::size_t>(len)); + } + + case 0xDF: // map 32 + { + uint32_t len; + return get_number(input_format_t::msgpack, len) and get_msgpack_object(static_cast<std::size_t>(len)); + } + + // negative fixint + case 0xE0: + case 0xE1: + case 0xE2: + case 0xE3: + case 0xE4: + case 0xE5: + case 0xE6: + case 0xE7: + case 0xE8: + case 0xE9: + case 0xEA: + case 0xEB: + case 0xEC: + case 0xED: + case 0xEE: + case 0xEF: + case 0xF0: + case 0xF1: + case 0xF2: + case 0xF3: + case 0xF4: + case 0xF5: + case 0xF6: + case 0xF7: + case 0xF8: + case 0xF9: + case 0xFA: + case 0xFB: + case 0xFC: + case 0xFD: + case 0xFE: + case 0xFF: + return sax->number_integer(static_cast<int8_t>(current)); + + default: // anything else + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format_t::msgpack, "invalid byte: 0x" + last_token, "value"))); + } + } + } + + /*! + @brief reads a MessagePack string + + This function first reads starting bytes to determine the expected + string length and then copies this number of bytes into a string. + + @param[out] result created string + + @return whether string creation completed + */ + bool get_msgpack_string(string_t& result) + { + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::msgpack, "string"))) + { + return false; + } + + switch (current) + { + // fixstr + case 0xA0: + case 0xA1: + case 0xA2: + case 0xA3: + case 0xA4: + case 0xA5: + case 0xA6: + case 0xA7: + case 0xA8: + case 0xA9: + case 0xAA: + case 0xAB: + case 0xAC: + case 0xAD: + case 0xAE: + case 0xAF: + case 0xB0: + case 0xB1: + case 0xB2: + case 0xB3: + case 0xB4: + case 0xB5: + case 0xB6: + case 0xB7: + case 0xB8: + case 0xB9: + case 0xBA: + case 0xBB: + case 0xBC: + case 0xBD: + case 0xBE: + case 0xBF: + { + return get_string(input_format_t::msgpack, current & 0x1F, result); + } + + case 0xD9: // str 8 + { + uint8_t len; + return get_number(input_format_t::msgpack, len) and get_string(input_format_t::msgpack, len, result); + } + + case 0xDA: // str 16 + { + uint16_t len; + return get_number(input_format_t::msgpack, len) and get_string(input_format_t::msgpack, len, result); + } + + case 0xDB: // str 32 + { + uint32_t len; + return get_number(input_format_t::msgpack, len) and get_string(input_format_t::msgpack, len, result); + } + + default: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format_t::msgpack, "expected length specification (0xA0-0xBF, 0xD9-0xDB); last byte: 0x" + last_token, "string"))); + } + } + } + + /*! + @param[in] len the length of the array + @return whether array creation completed + */ + bool get_msgpack_array(const std::size_t len) + { + if (JSON_UNLIKELY(not sax->start_array(len))) + { + return false; + } + + for (std::size_t i = 0; i < len; ++i) + { + if (JSON_UNLIKELY(not parse_msgpack_internal())) + { + return false; + } + } + + return sax->end_array(); + } + + /*! + @param[in] len the length of the object + @return whether object creation completed + */ + bool get_msgpack_object(const std::size_t len) + { + if (JSON_UNLIKELY(not sax->start_object(len))) + { + return false; + } + + string_t key; + for (std::size_t i = 0; i < len; ++i) + { + get(); + if (JSON_UNLIKELY(not get_msgpack_string(key) or not sax->key(key))) + { + return false; + } + + if (JSON_UNLIKELY(not parse_msgpack_internal())) + { + return false; + } + key.clear(); + } + + return sax->end_object(); + } + + //////////// + // UBJSON // + //////////// + + /*! + @param[in] get_char whether a new character should be retrieved from the + input (true, default) or whether the last read + character should be considered instead + + @return whether a valid UBJSON value was passed to the SAX parser + */ + bool parse_ubjson_internal(const bool get_char = true) + { + return get_ubjson_value(get_char ? get_ignore_noop() : current); + } + + /*! + @brief reads a UBJSON string + + This function is either called after reading the 'S' byte explicitly + indicating a string, or in case of an object key where the 'S' byte can be + left out. + + @param[out] result created string + @param[in] get_char whether a new character should be retrieved from the + input (true, default) or whether the last read + character should be considered instead + + @return whether string creation completed + */ + bool get_ubjson_string(string_t& result, const bool get_char = true) + { + if (get_char) + { + get(); // TODO: may we ignore N here? + } + + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::ubjson, "value"))) + { + return false; + } + + switch (current) + { + case 'U': + { + uint8_t len; + return get_number(input_format_t::ubjson, len) and get_string(input_format_t::ubjson, len, result); + } + + case 'i': + { + int8_t len; + return get_number(input_format_t::ubjson, len) and get_string(input_format_t::ubjson, len, result); + } + + case 'I': + { + int16_t len; + return get_number(input_format_t::ubjson, len) and get_string(input_format_t::ubjson, len, result); + } + + case 'l': + { + int32_t len; + return get_number(input_format_t::ubjson, len) and get_string(input_format_t::ubjson, len, result); + } + + case 'L': + { + int64_t len; + return get_number(input_format_t::ubjson, len) and get_string(input_format_t::ubjson, len, result); + } + + default: + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format_t::ubjson, "expected length type specification (U, i, I, l, L); last byte: 0x" + last_token, "string"))); + } + } + + /*! + @param[out] result determined size + @return whether size determination completed + */ + bool get_ubjson_size_value(std::size_t& result) + { + switch (get_ignore_noop()) + { + case 'U': + { + uint8_t number; + if (JSON_UNLIKELY(not get_number(input_format_t::ubjson, number))) + { + return false; + } + result = static_cast<std::size_t>(number); + return true; + } + + case 'i': + { + int8_t number; + if (JSON_UNLIKELY(not get_number(input_format_t::ubjson, number))) + { + return false; + } + result = static_cast<std::size_t>(number); + return true; + } + + case 'I': + { + int16_t number; + if (JSON_UNLIKELY(not get_number(input_format_t::ubjson, number))) + { + return false; + } + result = static_cast<std::size_t>(number); + return true; + } + + case 'l': + { + int32_t number; + if (JSON_UNLIKELY(not get_number(input_format_t::ubjson, number))) + { + return false; + } + result = static_cast<std::size_t>(number); + return true; + } + + case 'L': + { + int64_t number; + if (JSON_UNLIKELY(not get_number(input_format_t::ubjson, number))) + { + return false; + } + result = static_cast<std::size_t>(number); + return true; + } + + default: + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format_t::ubjson, "expected length type specification (U, i, I, l, L) after '#'; last byte: 0x" + last_token, "size"))); + } + } + } + + /*! + @brief determine the type and size for a container + + In the optimized UBJSON format, a type and a size can be provided to allow + for a more compact representation. + + @param[out] result pair of the size and the type + + @return whether pair creation completed + */ + bool get_ubjson_size_type(std::pair<std::size_t, int>& result) + { + result.first = string_t::npos; // size + result.second = 0; // type + + get_ignore_noop(); + + if (current == '$') + { + result.second = get(); // must not ignore 'N', because 'N' maybe the type + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::ubjson, "type"))) + { + return false; + } + + get_ignore_noop(); + if (JSON_UNLIKELY(current != '#')) + { + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::ubjson, "value"))) + { + return false; + } + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format_t::ubjson, "expected '#' after type information; last byte: 0x" + last_token, "size"))); + } + + return get_ubjson_size_value(result.first); + } + else if (current == '#') + { + return get_ubjson_size_value(result.first); + } + return true; + } + + /*! + @param prefix the previously read or set type prefix + @return whether value creation completed + */ + bool get_ubjson_value(const int prefix) + { + switch (prefix) + { + case std::char_traits<char>::eof(): // EOF + return unexpect_eof(input_format_t::ubjson, "value"); + + case 'T': // true + return sax->boolean(true); + case 'F': // false + return sax->boolean(false); + + case 'Z': // null + return sax->null(); + + case 'U': + { + uint8_t number; + return get_number(input_format_t::ubjson, number) and sax->number_unsigned(number); + } + + case 'i': + { + int8_t number; + return get_number(input_format_t::ubjson, number) and sax->number_integer(number); + } + + case 'I': + { + int16_t number; + return get_number(input_format_t::ubjson, number) and sax->number_integer(number); + } + + case 'l': + { + int32_t number; + return get_number(input_format_t::ubjson, number) and sax->number_integer(number); + } + + case 'L': + { + int64_t number; + return get_number(input_format_t::ubjson, number) and sax->number_integer(number); + } + + case 'd': + { + float number; + return get_number(input_format_t::ubjson, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + case 'D': + { + double number; + return get_number(input_format_t::ubjson, number) and sax->number_float(static_cast<number_float_t>(number), ""); + } + + case 'C': // char + { + get(); + if (JSON_UNLIKELY(not unexpect_eof(input_format_t::ubjson, "char"))) + { + return false; + } + if (JSON_UNLIKELY(current > 127)) + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(113, chars_read, exception_message(input_format_t::ubjson, "byte after 'C' must be in range 0x00..0x7F; last byte: 0x" + last_token, "char"))); + } + string_t s(1, static_cast<char>(current)); + return sax->string(s); + } + + case 'S': // string + { + string_t s; + return get_ubjson_string(s) and sax->string(s); + } + + case '[': // array + return get_ubjson_array(); + + case '{': // object + return get_ubjson_object(); + + default: // anything else + { + auto last_token = get_token_string(); + return sax->parse_error(chars_read, last_token, parse_error::create(112, chars_read, exception_message(input_format_t::ubjson, "invalid byte: 0x" + last_token, "value"))); + } + } + } + + /*! + @return whether array creation completed + */ + bool get_ubjson_array() + { + std::pair<std::size_t, int> size_and_type; + if (JSON_UNLIKELY(not get_ubjson_size_type(size_and_type))) + { + return false; + } + + if (size_and_type.first != string_t::npos) + { + if (JSON_UNLIKELY(not sax->start_array(size_and_type.first))) + { + return false; + } + + if (size_and_type.second != 0) + { + if (size_and_type.second != 'N') + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_UNLIKELY(not get_ubjson_value(size_and_type.second))) + { + return false; + } + } + } + } + else + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_UNLIKELY(not parse_ubjson_internal())) + { + return false; + } + } + } + } + else + { + if (JSON_UNLIKELY(not sax->start_array(std::size_t(-1)))) + { + return false; + } + + while (current != ']') + { + if (JSON_UNLIKELY(not parse_ubjson_internal(false))) + { + return false; + } + get_ignore_noop(); + } + } + + return sax->end_array(); + } + + /*! + @return whether object creation completed + */ + bool get_ubjson_object() + { + std::pair<std::size_t, int> size_and_type; + if (JSON_UNLIKELY(not get_ubjson_size_type(size_and_type))) + { + return false; + } + + string_t key; + if (size_and_type.first != string_t::npos) + { + if (JSON_UNLIKELY(not sax->start_object(size_and_type.first))) + { + return false; + } + + if (size_and_type.second != 0) + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_UNLIKELY(not get_ubjson_string(key) or not sax->key(key))) + { + return false; + } + if (JSON_UNLIKELY(not get_ubjson_value(size_and_type.second))) + { + return false; + } + key.clear(); + } + } + else + { + for (std::size_t i = 0; i < size_and_type.first; ++i) + { + if (JSON_UNLIKELY(not get_ubjson_string(key) or not sax->key(key))) + { + return false; + } + if (JSON_UNLIKELY(not parse_ubjson_internal())) + { + return false; + } + key.clear(); + } + } + } + else + { + if (JSON_UNLIKELY(not sax->start_object(std::size_t(-1)))) + { + return false; + } + + while (current != '}') + { + if (JSON_UNLIKELY(not get_ubjson_string(key, false) or not sax->key(key))) + { + return false; + } + if (JSON_UNLIKELY(not parse_ubjson_internal())) + { + return false; + } + get_ignore_noop(); + key.clear(); + } + } + + return sax->end_object(); + } + + /////////////////////// + // Utility functions // + /////////////////////// + + /*! + @brief get next character from the input + + This function provides the interface to the used input adapter. It does + not throw in case the input reached EOF, but returns a -'ve valued + `std::char_traits<char>::eof()` in that case. + + @return character read from the input + */ + int get() + { + ++chars_read; + return (current = ia->get_character()); + } + + /*! + @return character read from the input after ignoring all 'N' entries + */ + int get_ignore_noop() + { + do + { + get(); + } + while (current == 'N'); + + return current; + } + + /* + @brief read a number from the input + + @tparam NumberType the type of the number + @param[in] format the current format (for diagnostics) + @param[out] result number of type @a NumberType + + @return whether conversion completed + + @note This function needs to respect the system's endianess, because + bytes in CBOR, MessagePack, and UBJSON are stored in network order + (big endian) and therefore need reordering on little endian systems. + */ + template<typename NumberType, bool InputIsLittleEndian = false> + bool get_number(const input_format_t format, NumberType& result) + { + // step 1: read input into array with system's byte order + std::array<uint8_t, sizeof(NumberType)> vec; + for (std::size_t i = 0; i < sizeof(NumberType); ++i) + { + get(); + if (JSON_UNLIKELY(not unexpect_eof(format, "number"))) + { + return false; + } + + // reverse byte order prior to conversion if necessary + if (is_little_endian && !InputIsLittleEndian) + { + vec[sizeof(NumberType) - i - 1] = static_cast<uint8_t>(current); + } + else + { + vec[i] = static_cast<uint8_t>(current); // LCOV_EXCL_LINE + } + } + + // step 2: convert array into number of type T and return + std::memcpy(&result, vec.data(), sizeof(NumberType)); + return true; + } + + /*! + @brief create a string by reading characters from the input + + @tparam NumberType the type of the number + @param[in] format the current format (for diagnostics) + @param[in] len number of characters to read + @param[out] result string created by reading @a len bytes + + @return whether string creation completed + + @note We can not reserve @a len bytes for the result, because @a len + may be too large. Usually, @ref unexpect_eof() detects the end of + the input before we run out of string memory. + */ + template<typename NumberType> + bool get_string(const input_format_t format, + const NumberType len, + string_t& result) + { + bool success = true; + std::generate_n(std::back_inserter(result), len, [this, &success, &format]() + { + get(); + if (JSON_UNLIKELY(not unexpect_eof(format, "string"))) + { + success = false; + } + return static_cast<char>(current); + }); + return success; + } + + /*! + @param[in] format the current format (for diagnostics) + @param[in] context further context information (for diagnostics) + @return whether the last read character is not EOF + */ + bool unexpect_eof(const input_format_t format, const char* context) const + { + if (JSON_UNLIKELY(current == std::char_traits<char>::eof())) + { + return sax->parse_error(chars_read, "<end of file>", + parse_error::create(110, chars_read, exception_message(format, "unexpected end of input", context))); + } + return true; + } + + /*! + @return a string representation of the last read byte + */ + std::string get_token_string() const + { + char cr[3]; + snprintf(cr, 3, "%.2hhX", static_cast<unsigned char>(current)); + return std::string{cr}; + } + + /*! + @param[in] format the current format + @param[in] detail a detailed error message + @param[in] context further contect information + @return a message string to use in the parse_error exceptions + */ + std::string exception_message(const input_format_t format, + const std::string& detail, + const std::string& context) const + { + std::string error_msg = "syntax error while parsing "; + + switch (format) + { + case input_format_t::cbor: + error_msg += "CBOR"; + break; + + case input_format_t::msgpack: + error_msg += "MessagePack"; + break; + + case input_format_t::ubjson: + error_msg += "UBJSON"; + break; + + case input_format_t::bson: + error_msg += "BSON"; + break; + + // LCOV_EXCL_START + default: + assert(false); + // LCOV_EXCL_STOP + } + + return error_msg + " " + context + ": " + detail; + } + + private: + /// input adapter + input_adapter_t ia = nullptr; + + /// the current character + int current = std::char_traits<char>::eof(); + + /// the number of characters read + std::size_t chars_read = 0; + + /// whether we can assume little endianess + const bool is_little_endian = little_endianess(); + + /// the SAX parser + json_sax_t* sax = nullptr; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/output/binary_writer.hpp> + + +#include <algorithm> // reverse +#include <array> // array +#include <cstdint> // uint8_t, uint16_t, uint32_t, uint64_t +#include <cstring> // memcpy +#include <limits> // numeric_limits + +// #include <nlohmann/detail/input/binary_reader.hpp> + +// #include <nlohmann/detail/output/output_adapters.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/////////////////// +// binary writer // +/////////////////// + +/*! +@brief serialization to CBOR and MessagePack values +*/ +template<typename BasicJsonType, typename CharType> +class binary_writer +{ + using string_t = typename BasicJsonType::string_t; + + public: + /*! + @brief create a binary writer + + @param[in] adapter output adapter to write to + */ + explicit binary_writer(output_adapter_t<CharType> adapter) : oa(adapter) + { + assert(oa); + } + + /*! + @param[in] j JSON value to serialize + @pre j.type() == value_t::object + */ + void write_bson(const BasicJsonType& j) + { + switch (j.type()) + { + case value_t::object: + { + write_bson_object(*j.m_value.object); + break; + } + + default: + { + JSON_THROW(type_error::create(317, "to serialize to BSON, top-level type must be object, but is " + std::string(j.type_name()))); + } + } + } + + /*! + @param[in] j JSON value to serialize + */ + void write_cbor(const BasicJsonType& j) + { + switch (j.type()) + { + case value_t::null: + { + oa->write_character(to_char_type(0xF6)); + break; + } + + case value_t::boolean: + { + oa->write_character(j.m_value.boolean + ? to_char_type(0xF5) + : to_char_type(0xF4)); + break; + } + + case value_t::number_integer: + { + if (j.m_value.number_integer >= 0) + { + // CBOR does not differentiate between positive signed + // integers and unsigned integers. Therefore, we used the + // code from the value_t::number_unsigned case here. + if (j.m_value.number_integer <= 0x17) + { + write_number(static_cast<uint8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer <= (std::numeric_limits<uint8_t>::max)()) + { + oa->write_character(to_char_type(0x18)); + write_number(static_cast<uint8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer <= (std::numeric_limits<uint16_t>::max)()) + { + oa->write_character(to_char_type(0x19)); + write_number(static_cast<uint16_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer <= (std::numeric_limits<uint32_t>::max)()) + { + oa->write_character(to_char_type(0x1A)); + write_number(static_cast<uint32_t>(j.m_value.number_integer)); + } + else + { + oa->write_character(to_char_type(0x1B)); + write_number(static_cast<uint64_t>(j.m_value.number_integer)); + } + } + else + { + // The conversions below encode the sign in the first + // byte, and the value is converted to a positive number. + const auto positive_number = -1 - j.m_value.number_integer; + if (j.m_value.number_integer >= -24) + { + write_number(static_cast<uint8_t>(0x20 + positive_number)); + } + else if (positive_number <= (std::numeric_limits<uint8_t>::max)()) + { + oa->write_character(to_char_type(0x38)); + write_number(static_cast<uint8_t>(positive_number)); + } + else if (positive_number <= (std::numeric_limits<uint16_t>::max)()) + { + oa->write_character(to_char_type(0x39)); + write_number(static_cast<uint16_t>(positive_number)); + } + else if (positive_number <= (std::numeric_limits<uint32_t>::max)()) + { + oa->write_character(to_char_type(0x3A)); + write_number(static_cast<uint32_t>(positive_number)); + } + else + { + oa->write_character(to_char_type(0x3B)); + write_number(static_cast<uint64_t>(positive_number)); + } + } + break; + } + + case value_t::number_unsigned: + { + if (j.m_value.number_unsigned <= 0x17) + { + write_number(static_cast<uint8_t>(j.m_value.number_unsigned)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint8_t>::max)()) + { + oa->write_character(to_char_type(0x18)); + write_number(static_cast<uint8_t>(j.m_value.number_unsigned)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint16_t>::max)()) + { + oa->write_character(to_char_type(0x19)); + write_number(static_cast<uint16_t>(j.m_value.number_unsigned)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint32_t>::max)()) + { + oa->write_character(to_char_type(0x1A)); + write_number(static_cast<uint32_t>(j.m_value.number_unsigned)); + } + else + { + oa->write_character(to_char_type(0x1B)); + write_number(static_cast<uint64_t>(j.m_value.number_unsigned)); + } + break; + } + + case value_t::number_float: + { + oa->write_character(get_cbor_float_prefix(j.m_value.number_float)); + write_number(j.m_value.number_float); + break; + } + + case value_t::string: + { + // step 1: write control byte and the string length + const auto N = j.m_value.string->size(); + if (N <= 0x17) + { + write_number(static_cast<uint8_t>(0x60 + N)); + } + else if (N <= (std::numeric_limits<uint8_t>::max)()) + { + oa->write_character(to_char_type(0x78)); + write_number(static_cast<uint8_t>(N)); + } + else if (N <= (std::numeric_limits<uint16_t>::max)()) + { + oa->write_character(to_char_type(0x79)); + write_number(static_cast<uint16_t>(N)); + } + else if (N <= (std::numeric_limits<uint32_t>::max)()) + { + oa->write_character(to_char_type(0x7A)); + write_number(static_cast<uint32_t>(N)); + } + // LCOV_EXCL_START + else if (N <= (std::numeric_limits<uint64_t>::max)()) + { + oa->write_character(to_char_type(0x7B)); + write_number(static_cast<uint64_t>(N)); + } + // LCOV_EXCL_STOP + + // step 2: write the string + oa->write_characters( + reinterpret_cast<const CharType*>(j.m_value.string->c_str()), + j.m_value.string->size()); + break; + } + + case value_t::array: + { + // step 1: write control byte and the array size + const auto N = j.m_value.array->size(); + if (N <= 0x17) + { + write_number(static_cast<uint8_t>(0x80 + N)); + } + else if (N <= (std::numeric_limits<uint8_t>::max)()) + { + oa->write_character(to_char_type(0x98)); + write_number(static_cast<uint8_t>(N)); + } + else if (N <= (std::numeric_limits<uint16_t>::max)()) + { + oa->write_character(to_char_type(0x99)); + write_number(static_cast<uint16_t>(N)); + } + else if (N <= (std::numeric_limits<uint32_t>::max)()) + { + oa->write_character(to_char_type(0x9A)); + write_number(static_cast<uint32_t>(N)); + } + // LCOV_EXCL_START + else if (N <= (std::numeric_limits<uint64_t>::max)()) + { + oa->write_character(to_char_type(0x9B)); + write_number(static_cast<uint64_t>(N)); + } + // LCOV_EXCL_STOP + + // step 2: write each element + for (const auto& el : *j.m_value.array) + { + write_cbor(el); + } + break; + } + + case value_t::object: + { + // step 1: write control byte and the object size + const auto N = j.m_value.object->size(); + if (N <= 0x17) + { + write_number(static_cast<uint8_t>(0xA0 + N)); + } + else if (N <= (std::numeric_limits<uint8_t>::max)()) + { + oa->write_character(to_char_type(0xB8)); + write_number(static_cast<uint8_t>(N)); + } + else if (N <= (std::numeric_limits<uint16_t>::max)()) + { + oa->write_character(to_char_type(0xB9)); + write_number(static_cast<uint16_t>(N)); + } + else if (N <= (std::numeric_limits<uint32_t>::max)()) + { + oa->write_character(to_char_type(0xBA)); + write_number(static_cast<uint32_t>(N)); + } + // LCOV_EXCL_START + else if (N <= (std::numeric_limits<uint64_t>::max)()) + { + oa->write_character(to_char_type(0xBB)); + write_number(static_cast<uint64_t>(N)); + } + // LCOV_EXCL_STOP + + // step 2: write each element + for (const auto& el : *j.m_value.object) + { + write_cbor(el.first); + write_cbor(el.second); + } + break; + } + + default: + break; + } + } + + /*! + @param[in] j JSON value to serialize + */ + void write_msgpack(const BasicJsonType& j) + { + switch (j.type()) + { + case value_t::null: // nil + { + oa->write_character(to_char_type(0xC0)); + break; + } + + case value_t::boolean: // true and false + { + oa->write_character(j.m_value.boolean + ? to_char_type(0xC3) + : to_char_type(0xC2)); + break; + } + + case value_t::number_integer: + { + if (j.m_value.number_integer >= 0) + { + // MessagePack does not differentiate between positive + // signed integers and unsigned integers. Therefore, we used + // the code from the value_t::number_unsigned case here. + if (j.m_value.number_unsigned < 128) + { + // positive fixnum + write_number(static_cast<uint8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint8_t>::max)()) + { + // uint 8 + oa->write_character(to_char_type(0xCC)); + write_number(static_cast<uint8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint16_t>::max)()) + { + // uint 16 + oa->write_character(to_char_type(0xCD)); + write_number(static_cast<uint16_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint32_t>::max)()) + { + // uint 32 + oa->write_character(to_char_type(0xCE)); + write_number(static_cast<uint32_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint64_t>::max)()) + { + // uint 64 + oa->write_character(to_char_type(0xCF)); + write_number(static_cast<uint64_t>(j.m_value.number_integer)); + } + } + else + { + if (j.m_value.number_integer >= -32) + { + // negative fixnum + write_number(static_cast<int8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer >= (std::numeric_limits<int8_t>::min)() and + j.m_value.number_integer <= (std::numeric_limits<int8_t>::max)()) + { + // int 8 + oa->write_character(to_char_type(0xD0)); + write_number(static_cast<int8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer >= (std::numeric_limits<int16_t>::min)() and + j.m_value.number_integer <= (std::numeric_limits<int16_t>::max)()) + { + // int 16 + oa->write_character(to_char_type(0xD1)); + write_number(static_cast<int16_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer >= (std::numeric_limits<int32_t>::min)() and + j.m_value.number_integer <= (std::numeric_limits<int32_t>::max)()) + { + // int 32 + oa->write_character(to_char_type(0xD2)); + write_number(static_cast<int32_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_integer >= (std::numeric_limits<int64_t>::min)() and + j.m_value.number_integer <= (std::numeric_limits<int64_t>::max)()) + { + // int 64 + oa->write_character(to_char_type(0xD3)); + write_number(static_cast<int64_t>(j.m_value.number_integer)); + } + } + break; + } + + case value_t::number_unsigned: + { + if (j.m_value.number_unsigned < 128) + { + // positive fixnum + write_number(static_cast<uint8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint8_t>::max)()) + { + // uint 8 + oa->write_character(to_char_type(0xCC)); + write_number(static_cast<uint8_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint16_t>::max)()) + { + // uint 16 + oa->write_character(to_char_type(0xCD)); + write_number(static_cast<uint16_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint32_t>::max)()) + { + // uint 32 + oa->write_character(to_char_type(0xCE)); + write_number(static_cast<uint32_t>(j.m_value.number_integer)); + } + else if (j.m_value.number_unsigned <= (std::numeric_limits<uint64_t>::max)()) + { + // uint 64 + oa->write_character(to_char_type(0xCF)); + write_number(static_cast<uint64_t>(j.m_value.number_integer)); + } + break; + } + + case value_t::number_float: + { + oa->write_character(get_msgpack_float_prefix(j.m_value.number_float)); + write_number(j.m_value.number_float); + break; + } + + case value_t::string: + { + // step 1: write control byte and the string length + const auto N = j.m_value.string->size(); + if (N <= 31) + { + // fixstr + write_number(static_cast<uint8_t>(0xA0 | N)); + } + else if (N <= (std::numeric_limits<uint8_t>::max)()) + { + // str 8 + oa->write_character(to_char_type(0xD9)); + write_number(static_cast<uint8_t>(N)); + } + else if (N <= (std::numeric_limits<uint16_t>::max)()) + { + // str 16 + oa->write_character(to_char_type(0xDA)); + write_number(static_cast<uint16_t>(N)); + } + else if (N <= (std::numeric_limits<uint32_t>::max)()) + { + // str 32 + oa->write_character(to_char_type(0xDB)); + write_number(static_cast<uint32_t>(N)); + } + + // step 2: write the string + oa->write_characters( + reinterpret_cast<const CharType*>(j.m_value.string->c_str()), + j.m_value.string->size()); + break; + } + + case value_t::array: + { + // step 1: write control byte and the array size + const auto N = j.m_value.array->size(); + if (N <= 15) + { + // fixarray + write_number(static_cast<uint8_t>(0x90 | N)); + } + else if (N <= (std::numeric_limits<uint16_t>::max)()) + { + // array 16 + oa->write_character(to_char_type(0xDC)); + write_number(static_cast<uint16_t>(N)); + } + else if (N <= (std::numeric_limits<uint32_t>::max)()) + { + // array 32 + oa->write_character(to_char_type(0xDD)); + write_number(static_cast<uint32_t>(N)); + } + + // step 2: write each element + for (const auto& el : *j.m_value.array) + { + write_msgpack(el); + } + break; + } + + case value_t::object: + { + // step 1: write control byte and the object size + const auto N = j.m_value.object->size(); + if (N <= 15) + { + // fixmap + write_number(static_cast<uint8_t>(0x80 | (N & 0xF))); + } + else if (N <= (std::numeric_limits<uint16_t>::max)()) + { + // map 16 + oa->write_character(to_char_type(0xDE)); + write_number(static_cast<uint16_t>(N)); + } + else if (N <= (std::numeric_limits<uint32_t>::max)()) + { + // map 32 + oa->write_character(to_char_type(0xDF)); + write_number(static_cast<uint32_t>(N)); + } + + // step 2: write each element + for (const auto& el : *j.m_value.object) + { + write_msgpack(el.first); + write_msgpack(el.second); + } + break; + } + + default: + break; + } + } + + /*! + @param[in] j JSON value to serialize + @param[in] use_count whether to use '#' prefixes (optimized format) + @param[in] use_type whether to use '$' prefixes (optimized format) + @param[in] add_prefix whether prefixes need to be used for this value + */ + void write_ubjson(const BasicJsonType& j, const bool use_count, + const bool use_type, const bool add_prefix = true) + { + switch (j.type()) + { + case value_t::null: + { + if (add_prefix) + { + oa->write_character(to_char_type('Z')); + } + break; + } + + case value_t::boolean: + { + if (add_prefix) + { + oa->write_character(j.m_value.boolean + ? to_char_type('T') + : to_char_type('F')); + } + break; + } + + case value_t::number_integer: + { + write_number_with_ubjson_prefix(j.m_value.number_integer, add_prefix); + break; + } + + case value_t::number_unsigned: + { + write_number_with_ubjson_prefix(j.m_value.number_unsigned, add_prefix); + break; + } + + case value_t::number_float: + { + write_number_with_ubjson_prefix(j.m_value.number_float, add_prefix); + break; + } + + case value_t::string: + { + if (add_prefix) + { + oa->write_character(to_char_type('S')); + } + write_number_with_ubjson_prefix(j.m_value.string->size(), true); + oa->write_characters( + reinterpret_cast<const CharType*>(j.m_value.string->c_str()), + j.m_value.string->size()); + break; + } + + case value_t::array: + { + if (add_prefix) + { + oa->write_character(to_char_type('[')); + } + + bool prefix_required = true; + if (use_type and not j.m_value.array->empty()) + { + assert(use_count); + const CharType first_prefix = ubjson_prefix(j.front()); + const bool same_prefix = std::all_of(j.begin() + 1, j.end(), + [this, first_prefix](const BasicJsonType & v) + { + return ubjson_prefix(v) == first_prefix; + }); + + if (same_prefix) + { + prefix_required = false; + oa->write_character(to_char_type('$')); + oa->write_character(first_prefix); + } + } + + if (use_count) + { + oa->write_character(to_char_type('#')); + write_number_with_ubjson_prefix(j.m_value.array->size(), true); + } + + for (const auto& el : *j.m_value.array) + { + write_ubjson(el, use_count, use_type, prefix_required); + } + + if (not use_count) + { + oa->write_character(to_char_type(']')); + } + + break; + } + + case value_t::object: + { + if (add_prefix) + { + oa->write_character(to_char_type('{')); + } + + bool prefix_required = true; + if (use_type and not j.m_value.object->empty()) + { + assert(use_count); + const CharType first_prefix = ubjson_prefix(j.front()); + const bool same_prefix = std::all_of(j.begin(), j.end(), + [this, first_prefix](const BasicJsonType & v) + { + return ubjson_prefix(v) == first_prefix; + }); + + if (same_prefix) + { + prefix_required = false; + oa->write_character(to_char_type('$')); + oa->write_character(first_prefix); + } + } + + if (use_count) + { + oa->write_character(to_char_type('#')); + write_number_with_ubjson_prefix(j.m_value.object->size(), true); + } + + for (const auto& el : *j.m_value.object) + { + write_number_with_ubjson_prefix(el.first.size(), true); + oa->write_characters( + reinterpret_cast<const CharType*>(el.first.c_str()), + el.first.size()); + write_ubjson(el.second, use_count, use_type, prefix_required); + } + + if (not use_count) + { + oa->write_character(to_char_type('}')); + } + + break; + } + + default: + break; + } + } + + private: + ////////// + // BSON // + ////////// + + /*! + @return The size of a BSON document entry header, including the id marker + and the entry name size (and its null-terminator). + */ + static std::size_t calc_bson_entry_header_size(const string_t& name) + { + const auto it = name.find(static_cast<typename string_t::value_type>(0)); + if (JSON_UNLIKELY(it != BasicJsonType::string_t::npos)) + { + JSON_THROW(out_of_range::create(409, + "BSON key cannot contain code point U+0000 (at byte " + std::to_string(it) + ")")); + } + + return /*id*/ 1ul + name.size() + /*zero-terminator*/1u; + } + + /*! + @brief Writes the given @a element_type and @a name to the output adapter + */ + void write_bson_entry_header(const string_t& name, + const std::uint8_t element_type) + { + oa->write_character(to_char_type(element_type)); // boolean + oa->write_characters( + reinterpret_cast<const CharType*>(name.c_str()), + name.size() + 1u); + } + + /*! + @brief Writes a BSON element with key @a name and boolean value @a value + */ + void write_bson_boolean(const string_t& name, + const bool value) + { + write_bson_entry_header(name, 0x08); + oa->write_character(value ? to_char_type(0x01) : to_char_type(0x00)); + } + + /*! + @brief Writes a BSON element with key @a name and double value @a value + */ + void write_bson_double(const string_t& name, + const double value) + { + write_bson_entry_header(name, 0x01); + write_number<double, true>(value); + } + + /*! + @return The size of the BSON-encoded string in @a value + */ + static std::size_t calc_bson_string_size(const string_t& value) + { + return sizeof(std::int32_t) + value.size() + 1ul; + } + + /*! + @brief Writes a BSON element with key @a name and string value @a value + */ + void write_bson_string(const string_t& name, + const string_t& value) + { + write_bson_entry_header(name, 0x02); + + write_number<std::int32_t, true>(static_cast<std::int32_t>(value.size() + 1ul)); + oa->write_characters( + reinterpret_cast<const CharType*>(value.c_str()), + value.size() + 1); + } + + /*! + @brief Writes a BSON element with key @a name and null value + */ + void write_bson_null(const string_t& name) + { + write_bson_entry_header(name, 0x0A); + } + + /*! + @return The size of the BSON-encoded integer @a value + */ + static std::size_t calc_bson_integer_size(const std::int64_t value) + { + if ((std::numeric_limits<std::int32_t>::min)() <= value and value <= (std::numeric_limits<std::int32_t>::max)()) + { + return sizeof(std::int32_t); + } + else + { + return sizeof(std::int64_t); + } + } + + /*! + @brief Writes a BSON element with key @a name and integer @a value + */ + void write_bson_integer(const string_t& name, + const std::int64_t value) + { + if ((std::numeric_limits<std::int32_t>::min)() <= value and value <= (std::numeric_limits<std::int32_t>::max)()) + { + write_bson_entry_header(name, 0x10); // int32 + write_number<std::int32_t, true>(static_cast<std::int32_t>(value)); + } + else + { + write_bson_entry_header(name, 0x12); // int64 + write_number<std::int64_t, true>(static_cast<std::int64_t>(value)); + } + } + + /*! + @return The size of the BSON-encoded unsigned integer in @a j + */ + static constexpr std::size_t calc_bson_unsigned_size(const std::uint64_t value) noexcept + { + return (value <= static_cast<std::uint64_t>((std::numeric_limits<std::int32_t>::max)())) + ? sizeof(std::int32_t) + : sizeof(std::int64_t); + } + + /*! + @brief Writes a BSON element with key @a name and unsigned @a value + */ + void write_bson_unsigned(const string_t& name, + const std::uint64_t value) + { + if (value <= static_cast<std::uint64_t>((std::numeric_limits<std::int32_t>::max)())) + { + write_bson_entry_header(name, 0x10 /* int32 */); + write_number<std::int32_t, true>(static_cast<std::int32_t>(value)); + } + else if (value <= static_cast<std::uint64_t>((std::numeric_limits<std::int64_t>::max)())) + { + write_bson_entry_header(name, 0x12 /* int64 */); + write_number<std::int64_t, true>(static_cast<std::int64_t>(value)); + } + else + { + JSON_THROW(out_of_range::create(407, "integer number " + std::to_string(value) + " cannot be represented by BSON as it does not fit int64")); + } + } + + /*! + @brief Writes a BSON element with key @a name and object @a value + */ + void write_bson_object_entry(const string_t& name, + const typename BasicJsonType::object_t& value) + { + write_bson_entry_header(name, 0x03); // object + write_bson_object(value); + } + + /*! + @return The size of the BSON-encoded array @a value + */ + static std::size_t calc_bson_array_size(const typename BasicJsonType::array_t& value) + { + std::size_t embedded_document_size = 0ul; + std::size_t array_index = 0ul; + + for (const auto& el : value) + { + embedded_document_size += calc_bson_element_size(std::to_string(array_index++), el); + } + + return sizeof(std::int32_t) + embedded_document_size + 1ul; + } + + /*! + @brief Writes a BSON element with key @a name and array @a value + */ + void write_bson_array(const string_t& name, + const typename BasicJsonType::array_t& value) + { + write_bson_entry_header(name, 0x04); // array + write_number<std::int32_t, true>(static_cast<std::int32_t>(calc_bson_array_size(value))); + + std::size_t array_index = 0ul; + + for (const auto& el : value) + { + write_bson_element(std::to_string(array_index++), el); + } + + oa->write_character(to_char_type(0x00)); + } + + /*! + @brief Calculates the size necessary to serialize the JSON value @a j with its @a name + @return The calculated size for the BSON document entry for @a j with the given @a name. + */ + static std::size_t calc_bson_element_size(const string_t& name, + const BasicJsonType& j) + { + const auto header_size = calc_bson_entry_header_size(name); + switch (j.type()) + { + case value_t::object: + return header_size + calc_bson_object_size(*j.m_value.object); + + case value_t::array: + return header_size + calc_bson_array_size(*j.m_value.array); + + case value_t::boolean: + return header_size + 1ul; + + case value_t::number_float: + return header_size + 8ul; + + case value_t::number_integer: + return header_size + calc_bson_integer_size(j.m_value.number_integer); + + case value_t::number_unsigned: + return header_size + calc_bson_unsigned_size(j.m_value.number_unsigned); + + case value_t::string: + return header_size + calc_bson_string_size(*j.m_value.string); + + case value_t::null: + return header_size + 0ul; + + // LCOV_EXCL_START + default: + assert(false); + return 0ul; + // LCOV_EXCL_STOP + }; + } + + /*! + @brief Serializes the JSON value @a j to BSON and associates it with the + key @a name. + @param name The name to associate with the JSON entity @a j within the + current BSON document + @return The size of the BSON entry + */ + void write_bson_element(const string_t& name, + const BasicJsonType& j) + { + switch (j.type()) + { + case value_t::object: + return write_bson_object_entry(name, *j.m_value.object); + + case value_t::array: + return write_bson_array(name, *j.m_value.array); + + case value_t::boolean: + return write_bson_boolean(name, j.m_value.boolean); + + case value_t::number_float: + return write_bson_double(name, j.m_value.number_float); + + case value_t::number_integer: + return write_bson_integer(name, j.m_value.number_integer); + + case value_t::number_unsigned: + return write_bson_unsigned(name, j.m_value.number_unsigned); + + case value_t::string: + return write_bson_string(name, *j.m_value.string); + + case value_t::null: + return write_bson_null(name); + + // LCOV_EXCL_START + default: + assert(false); + return; + // LCOV_EXCL_STOP + }; + } + + /*! + @brief Calculates the size of the BSON serialization of the given + JSON-object @a j. + @param[in] j JSON value to serialize + @pre j.type() == value_t::object + */ + static std::size_t calc_bson_object_size(const typename BasicJsonType::object_t& value) + { + std::size_t document_size = std::accumulate(value.begin(), value.end(), 0ul, + [](size_t result, const typename BasicJsonType::object_t::value_type & el) + { + return result += calc_bson_element_size(el.first, el.second); + }); + + return sizeof(std::int32_t) + document_size + 1ul; + } + + /*! + @param[in] j JSON value to serialize + @pre j.type() == value_t::object + */ + void write_bson_object(const typename BasicJsonType::object_t& value) + { + write_number<std::int32_t, true>(static_cast<std::int32_t>(calc_bson_object_size(value))); + + for (const auto& el : value) + { + write_bson_element(el.first, el.second); + } + + oa->write_character(to_char_type(0x00)); + } + + ////////// + // CBOR // + ////////// + + static constexpr CharType get_cbor_float_prefix(float /*unused*/) + { + return to_char_type(0xFA); // Single-Precision Float + } + + static constexpr CharType get_cbor_float_prefix(double /*unused*/) + { + return to_char_type(0xFB); // Double-Precision Float + } + + ///////////// + // MsgPack // + ///////////// + + static constexpr CharType get_msgpack_float_prefix(float /*unused*/) + { + return to_char_type(0xCA); // float 32 + } + + static constexpr CharType get_msgpack_float_prefix(double /*unused*/) + { + return to_char_type(0xCB); // float 64 + } + + //////////// + // UBJSON // + //////////// + + // UBJSON: write number (floating point) + template<typename NumberType, typename std::enable_if< + std::is_floating_point<NumberType>::value, int>::type = 0> + void write_number_with_ubjson_prefix(const NumberType n, + const bool add_prefix) + { + if (add_prefix) + { + oa->write_character(get_ubjson_float_prefix(n)); + } + write_number(n); + } + + // UBJSON: write number (unsigned integer) + template<typename NumberType, typename std::enable_if< + std::is_unsigned<NumberType>::value, int>::type = 0> + void write_number_with_ubjson_prefix(const NumberType n, + const bool add_prefix) + { + if (n <= static_cast<uint64_t>((std::numeric_limits<int8_t>::max)())) + { + if (add_prefix) + { + oa->write_character(to_char_type('i')); // int8 + } + write_number(static_cast<uint8_t>(n)); + } + else if (n <= (std::numeric_limits<uint8_t>::max)()) + { + if (add_prefix) + { + oa->write_character(to_char_type('U')); // uint8 + } + write_number(static_cast<uint8_t>(n)); + } + else if (n <= static_cast<uint64_t>((std::numeric_limits<int16_t>::max)())) + { + if (add_prefix) + { + oa->write_character(to_char_type('I')); // int16 + } + write_number(static_cast<int16_t>(n)); + } + else if (n <= static_cast<uint64_t>((std::numeric_limits<int32_t>::max)())) + { + if (add_prefix) + { + oa->write_character(to_char_type('l')); // int32 + } + write_number(static_cast<int32_t>(n)); + } + else if (n <= static_cast<uint64_t>((std::numeric_limits<int64_t>::max)())) + { + if (add_prefix) + { + oa->write_character(to_char_type('L')); // int64 + } + write_number(static_cast<int64_t>(n)); + } + else + { + JSON_THROW(out_of_range::create(407, "integer number " + std::to_string(n) + " cannot be represented by UBJSON as it does not fit int64")); + } + } + + // UBJSON: write number (signed integer) + template<typename NumberType, typename std::enable_if< + std::is_signed<NumberType>::value and + not std::is_floating_point<NumberType>::value, int>::type = 0> + void write_number_with_ubjson_prefix(const NumberType n, + const bool add_prefix) + { + if ((std::numeric_limits<int8_t>::min)() <= n and n <= (std::numeric_limits<int8_t>::max)()) + { + if (add_prefix) + { + oa->write_character(to_char_type('i')); // int8 + } + write_number(static_cast<int8_t>(n)); + } + else if (static_cast<int64_t>((std::numeric_limits<uint8_t>::min)()) <= n and n <= static_cast<int64_t>((std::numeric_limits<uint8_t>::max)())) + { + if (add_prefix) + { + oa->write_character(to_char_type('U')); // uint8 + } + write_number(static_cast<uint8_t>(n)); + } + else if ((std::numeric_limits<int16_t>::min)() <= n and n <= (std::numeric_limits<int16_t>::max)()) + { + if (add_prefix) + { + oa->write_character(to_char_type('I')); // int16 + } + write_number(static_cast<int16_t>(n)); + } + else if ((std::numeric_limits<int32_t>::min)() <= n and n <= (std::numeric_limits<int32_t>::max)()) + { + if (add_prefix) + { + oa->write_character(to_char_type('l')); // int32 + } + write_number(static_cast<int32_t>(n)); + } + else if ((std::numeric_limits<int64_t>::min)() <= n and n <= (std::numeric_limits<int64_t>::max)()) + { + if (add_prefix) + { + oa->write_character(to_char_type('L')); // int64 + } + write_number(static_cast<int64_t>(n)); + } + // LCOV_EXCL_START + else + { + JSON_THROW(out_of_range::create(407, "integer number " + std::to_string(n) + " cannot be represented by UBJSON as it does not fit int64")); + } + // LCOV_EXCL_STOP + } + + /*! + @brief determine the type prefix of container values + + @note This function does not need to be 100% accurate when it comes to + integer limits. In case a number exceeds the limits of int64_t, + this will be detected by a later call to function + write_number_with_ubjson_prefix. Therefore, we return 'L' for any + value that does not fit the previous limits. + */ + CharType ubjson_prefix(const BasicJsonType& j) const noexcept + { + switch (j.type()) + { + case value_t::null: + return 'Z'; + + case value_t::boolean: + return j.m_value.boolean ? 'T' : 'F'; + + case value_t::number_integer: + { + if ((std::numeric_limits<int8_t>::min)() <= j.m_value.number_integer and j.m_value.number_integer <= (std::numeric_limits<int8_t>::max)()) + { + return 'i'; + } + if ((std::numeric_limits<uint8_t>::min)() <= j.m_value.number_integer and j.m_value.number_integer <= (std::numeric_limits<uint8_t>::max)()) + { + return 'U'; + } + if ((std::numeric_limits<int16_t>::min)() <= j.m_value.number_integer and j.m_value.number_integer <= (std::numeric_limits<int16_t>::max)()) + { + return 'I'; + } + if ((std::numeric_limits<int32_t>::min)() <= j.m_value.number_integer and j.m_value.number_integer <= (std::numeric_limits<int32_t>::max)()) + { + return 'l'; + } + // no check and assume int64_t (see note above) + return 'L'; + } + + case value_t::number_unsigned: + { + if (j.m_value.number_unsigned <= (std::numeric_limits<int8_t>::max)()) + { + return 'i'; + } + if (j.m_value.number_unsigned <= (std::numeric_limits<uint8_t>::max)()) + { + return 'U'; + } + if (j.m_value.number_unsigned <= (std::numeric_limits<int16_t>::max)()) + { + return 'I'; + } + if (j.m_value.number_unsigned <= (std::numeric_limits<int32_t>::max)()) + { + return 'l'; + } + // no check and assume int64_t (see note above) + return 'L'; + } + + case value_t::number_float: + return get_ubjson_float_prefix(j.m_value.number_float); + + case value_t::string: + return 'S'; + + case value_t::array: + return '['; + + case value_t::object: + return '{'; + + default: // discarded values + return 'N'; + } + } + + static constexpr CharType get_ubjson_float_prefix(float /*unused*/) + { + return 'd'; // float 32 + } + + static constexpr CharType get_ubjson_float_prefix(double /*unused*/) + { + return 'D'; // float 64 + } + + /////////////////////// + // Utility functions // + /////////////////////// + + /* + @brief write a number to output input + @param[in] n number of type @a NumberType + @tparam NumberType the type of the number + @tparam OutputIsLittleEndian Set to true if output data is + required to be little endian + + @note This function needs to respect the system's endianess, because bytes + in CBOR, MessagePack, and UBJSON are stored in network order (big + endian) and therefore need reordering on little endian systems. + */ + template<typename NumberType, bool OutputIsLittleEndian = false> + void write_number(const NumberType n) + { + // step 1: write number to array of length NumberType + std::array<CharType, sizeof(NumberType)> vec; + std::memcpy(vec.data(), &n, sizeof(NumberType)); + + // step 2: write array to output (with possible reordering) + if (is_little_endian and not OutputIsLittleEndian) + { + // reverse byte order prior to conversion if necessary + std::reverse(vec.begin(), vec.end()); + } + + oa->write_characters(vec.data(), sizeof(NumberType)); + } + + public: + // The following to_char_type functions are implement the conversion + // between uint8_t and CharType. In case CharType is not unsigned, + // such a conversion is required to allow values greater than 128. + // See <https://github.com/nlohmann/json/issues/1286> for a discussion. + template < typename C = CharType, + enable_if_t < std::is_signed<C>::value and std::is_signed<char>::value > * = nullptr > + static constexpr CharType to_char_type(std::uint8_t x) noexcept + { + return *reinterpret_cast<char*>(&x); + } + + template < typename C = CharType, + enable_if_t < std::is_signed<C>::value and std::is_unsigned<char>::value > * = nullptr > + static CharType to_char_type(std::uint8_t x) noexcept + { + static_assert(sizeof(std::uint8_t) == sizeof(CharType), "size of CharType must be equal to std::uint8_t"); + static_assert(std::is_pod<CharType>::value, "CharType must be POD"); + CharType result; + std::memcpy(&result, &x, sizeof(x)); + return result; + } + + template<typename C = CharType, + enable_if_t<std::is_unsigned<C>::value>* = nullptr> + static constexpr CharType to_char_type(std::uint8_t x) noexcept + { + return x; + } + + template < typename InputCharType, typename C = CharType, + enable_if_t < + std::is_signed<C>::value and + std::is_signed<char>::value and + std::is_same<char, typename std::remove_cv<InputCharType>::type>::value + > * = nullptr > + static constexpr CharType to_char_type(InputCharType x) noexcept + { + return x; + } + + private: + /// whether we can assume little endianess + const bool is_little_endian = binary_reader<BasicJsonType>::little_endianess(); + + /// the output + output_adapter_t<CharType> oa = nullptr; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/output/serializer.hpp> + + +#include <algorithm> // reverse, remove, fill, find, none_of +#include <array> // array +#include <cassert> // assert +#include <ciso646> // and, or +#include <clocale> // localeconv, lconv +#include <cmath> // labs, isfinite, isnan, signbit +#include <cstddef> // size_t, ptrdiff_t +#include <cstdint> // uint8_t +#include <cstdio> // snprintf +#include <limits> // numeric_limits +#include <string> // string +#include <type_traits> // is_same + +// #include <nlohmann/detail/exceptions.hpp> + +// #include <nlohmann/detail/conversions/to_chars.hpp> + + +#include <cassert> // assert +#include <ciso646> // or, and, not +#include <cmath> // signbit, isfinite +#include <cstdint> // intN_t, uintN_t +#include <cstring> // memcpy, memmove + +namespace nlohmann +{ +namespace detail +{ + +/*! +@brief implements the Grisu2 algorithm for binary to decimal floating-point +conversion. + +This implementation is a slightly modified version of the reference +implementation which may be obtained from +http://florian.loitsch.com/publications (bench.tar.gz). + +The code is distributed under the MIT license, Copyright (c) 2009 Florian Loitsch. + +For a detailed description of the algorithm see: + +[1] Loitsch, "Printing Floating-Point Numbers Quickly and Accurately with + Integers", Proceedings of the ACM SIGPLAN 2010 Conference on Programming + Language Design and Implementation, PLDI 2010 +[2] Burger, Dybvig, "Printing Floating-Point Numbers Quickly and Accurately", + Proceedings of the ACM SIGPLAN 1996 Conference on Programming Language + Design and Implementation, PLDI 1996 +*/ +namespace dtoa_impl +{ + +template <typename Target, typename Source> +Target reinterpret_bits(const Source source) +{ + static_assert(sizeof(Target) == sizeof(Source), "size mismatch"); + + Target target; + std::memcpy(&target, &source, sizeof(Source)); + return target; +} + +struct diyfp // f * 2^e +{ + static constexpr int kPrecision = 64; // = q + + uint64_t f = 0; + int e = 0; + + constexpr diyfp(uint64_t f_, int e_) noexcept : f(f_), e(e_) {} + + /*! + @brief returns x - y + @pre x.e == y.e and x.f >= y.f + */ + static diyfp sub(const diyfp& x, const diyfp& y) noexcept + { + assert(x.e == y.e); + assert(x.f >= y.f); + + return {x.f - y.f, x.e}; + } + + /*! + @brief returns x * y + @note The result is rounded. (Only the upper q bits are returned.) + */ + static diyfp mul(const diyfp& x, const diyfp& y) noexcept + { + static_assert(kPrecision == 64, "internal error"); + + // Computes: + // f = round((x.f * y.f) / 2^q) + // e = x.e + y.e + q + + // Emulate the 64-bit * 64-bit multiplication: + // + // p = u * v + // = (u_lo + 2^32 u_hi) (v_lo + 2^32 v_hi) + // = (u_lo v_lo ) + 2^32 ((u_lo v_hi ) + (u_hi v_lo )) + 2^64 (u_hi v_hi ) + // = (p0 ) + 2^32 ((p1 ) + (p2 )) + 2^64 (p3 ) + // = (p0_lo + 2^32 p0_hi) + 2^32 ((p1_lo + 2^32 p1_hi) + (p2_lo + 2^32 p2_hi)) + 2^64 (p3 ) + // = (p0_lo ) + 2^32 (p0_hi + p1_lo + p2_lo ) + 2^64 (p1_hi + p2_hi + p3) + // = (p0_lo ) + 2^32 (Q ) + 2^64 (H ) + // = (p0_lo ) + 2^32 (Q_lo + 2^32 Q_hi ) + 2^64 (H ) + // + // (Since Q might be larger than 2^32 - 1) + // + // = (p0_lo + 2^32 Q_lo) + 2^64 (Q_hi + H) + // + // (Q_hi + H does not overflow a 64-bit int) + // + // = p_lo + 2^64 p_hi + + const uint64_t u_lo = x.f & 0xFFFFFFFF; + const uint64_t u_hi = x.f >> 32; + const uint64_t v_lo = y.f & 0xFFFFFFFF; + const uint64_t v_hi = y.f >> 32; + + const uint64_t p0 = u_lo * v_lo; + const uint64_t p1 = u_lo * v_hi; + const uint64_t p2 = u_hi * v_lo; + const uint64_t p3 = u_hi * v_hi; + + const uint64_t p0_hi = p0 >> 32; + const uint64_t p1_lo = p1 & 0xFFFFFFFF; + const uint64_t p1_hi = p1 >> 32; + const uint64_t p2_lo = p2 & 0xFFFFFFFF; + const uint64_t p2_hi = p2 >> 32; + + uint64_t Q = p0_hi + p1_lo + p2_lo; + + // The full product might now be computed as + // + // p_hi = p3 + p2_hi + p1_hi + (Q >> 32) + // p_lo = p0_lo + (Q << 32) + // + // But in this particular case here, the full p_lo is not required. + // Effectively we only need to add the highest bit in p_lo to p_hi (and + // Q_hi + 1 does not overflow). + + Q += uint64_t{1} << (64 - 32 - 1); // round, ties up + + const uint64_t h = p3 + p2_hi + p1_hi + (Q >> 32); + + return {h, x.e + y.e + 64}; + } + + /*! + @brief normalize x such that the significand is >= 2^(q-1) + @pre x.f != 0 + */ + static diyfp normalize(diyfp x) noexcept + { + assert(x.f != 0); + + while ((x.f >> 63) == 0) + { + x.f <<= 1; + x.e--; + } + + return x; + } + + /*! + @brief normalize x such that the result has the exponent E + @pre e >= x.e and the upper e - x.e bits of x.f must be zero. + */ + static diyfp normalize_to(const diyfp& x, const int target_exponent) noexcept + { + const int delta = x.e - target_exponent; + + assert(delta >= 0); + assert(((x.f << delta) >> delta) == x.f); + + return {x.f << delta, target_exponent}; + } +}; + +struct boundaries +{ + diyfp w; + diyfp minus; + diyfp plus; +}; + +/*! +Compute the (normalized) diyfp representing the input number 'value' and its +boundaries. + +@pre value must be finite and positive +*/ +template <typename FloatType> +boundaries compute_boundaries(FloatType value) +{ + assert(std::isfinite(value)); + assert(value > 0); + + // Convert the IEEE representation into a diyfp. + // + // If v is denormal: + // value = 0.F * 2^(1 - bias) = ( F) * 2^(1 - bias - (p-1)) + // If v is normalized: + // value = 1.F * 2^(E - bias) = (2^(p-1) + F) * 2^(E - bias - (p-1)) + + static_assert(std::numeric_limits<FloatType>::is_iec559, + "internal error: dtoa_short requires an IEEE-754 floating-point implementation"); + + constexpr int kPrecision = std::numeric_limits<FloatType>::digits; // = p (includes the hidden bit) + constexpr int kBias = std::numeric_limits<FloatType>::max_exponent - 1 + (kPrecision - 1); + constexpr int kMinExp = 1 - kBias; + constexpr uint64_t kHiddenBit = uint64_t{1} << (kPrecision - 1); // = 2^(p-1) + + using bits_type = typename std::conditional< kPrecision == 24, uint32_t, uint64_t >::type; + + const uint64_t bits = reinterpret_bits<bits_type>(value); + const uint64_t E = bits >> (kPrecision - 1); + const uint64_t F = bits & (kHiddenBit - 1); + + const bool is_denormal = (E == 0); + const diyfp v = is_denormal + ? diyfp(F, kMinExp) + : diyfp(F + kHiddenBit, static_cast<int>(E) - kBias); + + // Compute the boundaries m- and m+ of the floating-point value + // v = f * 2^e. + // + // Determine v- and v+, the floating-point predecessor and successor if v, + // respectively. + // + // v- = v - 2^e if f != 2^(p-1) or e == e_min (A) + // = v - 2^(e-1) if f == 2^(p-1) and e > e_min (B) + // + // v+ = v + 2^e + // + // Let m- = (v- + v) / 2 and m+ = (v + v+) / 2. All real numbers _strictly_ + // between m- and m+ round to v, regardless of how the input rounding + // algorithm breaks ties. + // + // ---+-------------+-------------+-------------+-------------+--- (A) + // v- m- v m+ v+ + // + // -----------------+------+------+-------------+-------------+--- (B) + // v- m- v m+ v+ + + const bool lower_boundary_is_closer = (F == 0 and E > 1); + const diyfp m_plus = diyfp(2 * v.f + 1, v.e - 1); + const diyfp m_minus = lower_boundary_is_closer + ? diyfp(4 * v.f - 1, v.e - 2) // (B) + : diyfp(2 * v.f - 1, v.e - 1); // (A) + + // Determine the normalized w+ = m+. + const diyfp w_plus = diyfp::normalize(m_plus); + + // Determine w- = m- such that e_(w-) = e_(w+). + const diyfp w_minus = diyfp::normalize_to(m_minus, w_plus.e); + + return {diyfp::normalize(v), w_minus, w_plus}; +} + +// Given normalized diyfp w, Grisu needs to find a (normalized) cached +// power-of-ten c, such that the exponent of the product c * w = f * 2^e lies +// within a certain range [alpha, gamma] (Definition 3.2 from [1]) +// +// alpha <= e = e_c + e_w + q <= gamma +// +// or +// +// f_c * f_w * 2^alpha <= f_c 2^(e_c) * f_w 2^(e_w) * 2^q +// <= f_c * f_w * 2^gamma +// +// Since c and w are normalized, i.e. 2^(q-1) <= f < 2^q, this implies +// +// 2^(q-1) * 2^(q-1) * 2^alpha <= c * w * 2^q < 2^q * 2^q * 2^gamma +// +// or +// +// 2^(q - 2 + alpha) <= c * w < 2^(q + gamma) +// +// The choice of (alpha,gamma) determines the size of the table and the form of +// the digit generation procedure. Using (alpha,gamma)=(-60,-32) works out well +// in practice: +// +// The idea is to cut the number c * w = f * 2^e into two parts, which can be +// processed independently: An integral part p1, and a fractional part p2: +// +// f * 2^e = ( (f div 2^-e) * 2^-e + (f mod 2^-e) ) * 2^e +// = (f div 2^-e) + (f mod 2^-e) * 2^e +// = p1 + p2 * 2^e +// +// The conversion of p1 into decimal form requires a series of divisions and +// modulos by (a power of) 10. These operations are faster for 32-bit than for +// 64-bit integers, so p1 should ideally fit into a 32-bit integer. This can be +// achieved by choosing +// +// -e >= 32 or e <= -32 := gamma +// +// In order to convert the fractional part +// +// p2 * 2^e = p2 / 2^-e = d[-1] / 10^1 + d[-2] / 10^2 + ... +// +// into decimal form, the fraction is repeatedly multiplied by 10 and the digits +// d[-i] are extracted in order: +// +// (10 * p2) div 2^-e = d[-1] +// (10 * p2) mod 2^-e = d[-2] / 10^1 + ... +// +// The multiplication by 10 must not overflow. It is sufficient to choose +// +// 10 * p2 < 16 * p2 = 2^4 * p2 <= 2^64. +// +// Since p2 = f mod 2^-e < 2^-e, +// +// -e <= 60 or e >= -60 := alpha + +constexpr int kAlpha = -60; +constexpr int kGamma = -32; + +struct cached_power // c = f * 2^e ~= 10^k +{ + uint64_t f; + int e; + int k; +}; + +/*! +For a normalized diyfp w = f * 2^e, this function returns a (normalized) cached +power-of-ten c = f_c * 2^e_c, such that the exponent of the product w * c +satisfies (Definition 3.2 from [1]) + + alpha <= e_c + e + q <= gamma. +*/ +inline cached_power get_cached_power_for_binary_exponent(int e) +{ + // Now + // + // alpha <= e_c + e + q <= gamma (1) + // ==> f_c * 2^alpha <= c * 2^e * 2^q + // + // and since the c's are normalized, 2^(q-1) <= f_c, + // + // ==> 2^(q - 1 + alpha) <= c * 2^(e + q) + // ==> 2^(alpha - e - 1) <= c + // + // If c were an exakt power of ten, i.e. c = 10^k, one may determine k as + // + // k = ceil( log_10( 2^(alpha - e - 1) ) ) + // = ceil( (alpha - e - 1) * log_10(2) ) + // + // From the paper: + // "In theory the result of the procedure could be wrong since c is rounded, + // and the computation itself is approximated [...]. In practice, however, + // this simple function is sufficient." + // + // For IEEE double precision floating-point numbers converted into + // normalized diyfp's w = f * 2^e, with q = 64, + // + // e >= -1022 (min IEEE exponent) + // -52 (p - 1) + // -52 (p - 1, possibly normalize denormal IEEE numbers) + // -11 (normalize the diyfp) + // = -1137 + // + // and + // + // e <= +1023 (max IEEE exponent) + // -52 (p - 1) + // -11 (normalize the diyfp) + // = 960 + // + // This binary exponent range [-1137,960] results in a decimal exponent + // range [-307,324]. One does not need to store a cached power for each + // k in this range. For each such k it suffices to find a cached power + // such that the exponent of the product lies in [alpha,gamma]. + // This implies that the difference of the decimal exponents of adjacent + // table entries must be less than or equal to + // + // floor( (gamma - alpha) * log_10(2) ) = 8. + // + // (A smaller distance gamma-alpha would require a larger table.) + + // NB: + // Actually this function returns c, such that -60 <= e_c + e + 64 <= -34. + + constexpr int kCachedPowersSize = 79; + constexpr int kCachedPowersMinDecExp = -300; + constexpr int kCachedPowersDecStep = 8; + + static constexpr cached_power kCachedPowers[] = + { + { 0xAB70FE17C79AC6CA, -1060, -300 }, + { 0xFF77B1FCBEBCDC4F, -1034, -292 }, + { 0xBE5691EF416BD60C, -1007, -284 }, + { 0x8DD01FAD907FFC3C, -980, -276 }, + { 0xD3515C2831559A83, -954, -268 }, + { 0x9D71AC8FADA6C9B5, -927, -260 }, + { 0xEA9C227723EE8BCB, -901, -252 }, + { 0xAECC49914078536D, -874, -244 }, + { 0x823C12795DB6CE57, -847, -236 }, + { 0xC21094364DFB5637, -821, -228 }, + { 0x9096EA6F3848984F, -794, -220 }, + { 0xD77485CB25823AC7, -768, -212 }, + { 0xA086CFCD97BF97F4, -741, -204 }, + { 0xEF340A98172AACE5, -715, -196 }, + { 0xB23867FB2A35B28E, -688, -188 }, + { 0x84C8D4DFD2C63F3B, -661, -180 }, + { 0xC5DD44271AD3CDBA, -635, -172 }, + { 0x936B9FCEBB25C996, -608, -164 }, + { 0xDBAC6C247D62A584, -582, -156 }, + { 0xA3AB66580D5FDAF6, -555, -148 }, + { 0xF3E2F893DEC3F126, -529, -140 }, + { 0xB5B5ADA8AAFF80B8, -502, -132 }, + { 0x87625F056C7C4A8B, -475, -124 }, + { 0xC9BCFF6034C13053, -449, -116 }, + { 0x964E858C91BA2655, -422, -108 }, + { 0xDFF9772470297EBD, -396, -100 }, + { 0xA6DFBD9FB8E5B88F, -369, -92 }, + { 0xF8A95FCF88747D94, -343, -84 }, + { 0xB94470938FA89BCF, -316, -76 }, + { 0x8A08F0F8BF0F156B, -289, -68 }, + { 0xCDB02555653131B6, -263, -60 }, + { 0x993FE2C6D07B7FAC, -236, -52 }, + { 0xE45C10C42A2B3B06, -210, -44 }, + { 0xAA242499697392D3, -183, -36 }, + { 0xFD87B5F28300CA0E, -157, -28 }, + { 0xBCE5086492111AEB, -130, -20 }, + { 0x8CBCCC096F5088CC, -103, -12 }, + { 0xD1B71758E219652C, -77, -4 }, + { 0x9C40000000000000, -50, 4 }, + { 0xE8D4A51000000000, -24, 12 }, + { 0xAD78EBC5AC620000, 3, 20 }, + { 0x813F3978F8940984, 30, 28 }, + { 0xC097CE7BC90715B3, 56, 36 }, + { 0x8F7E32CE7BEA5C70, 83, 44 }, + { 0xD5D238A4ABE98068, 109, 52 }, + { 0x9F4F2726179A2245, 136, 60 }, + { 0xED63A231D4C4FB27, 162, 68 }, + { 0xB0DE65388CC8ADA8, 189, 76 }, + { 0x83C7088E1AAB65DB, 216, 84 }, + { 0xC45D1DF942711D9A, 242, 92 }, + { 0x924D692CA61BE758, 269, 100 }, + { 0xDA01EE641A708DEA, 295, 108 }, + { 0xA26DA3999AEF774A, 322, 116 }, + { 0xF209787BB47D6B85, 348, 124 }, + { 0xB454E4A179DD1877, 375, 132 }, + { 0x865B86925B9BC5C2, 402, 140 }, + { 0xC83553C5C8965D3D, 428, 148 }, + { 0x952AB45CFA97A0B3, 455, 156 }, + { 0xDE469FBD99A05FE3, 481, 164 }, + { 0xA59BC234DB398C25, 508, 172 }, + { 0xF6C69A72A3989F5C, 534, 180 }, + { 0xB7DCBF5354E9BECE, 561, 188 }, + { 0x88FCF317F22241E2, 588, 196 }, + { 0xCC20CE9BD35C78A5, 614, 204 }, + { 0x98165AF37B2153DF, 641, 212 }, + { 0xE2A0B5DC971F303A, 667, 220 }, + { 0xA8D9D1535CE3B396, 694, 228 }, + { 0xFB9B7CD9A4A7443C, 720, 236 }, + { 0xBB764C4CA7A44410, 747, 244 }, + { 0x8BAB8EEFB6409C1A, 774, 252 }, + { 0xD01FEF10A657842C, 800, 260 }, + { 0x9B10A4E5E9913129, 827, 268 }, + { 0xE7109BFBA19C0C9D, 853, 276 }, + { 0xAC2820D9623BF429, 880, 284 }, + { 0x80444B5E7AA7CF85, 907, 292 }, + { 0xBF21E44003ACDD2D, 933, 300 }, + { 0x8E679C2F5E44FF8F, 960, 308 }, + { 0xD433179D9C8CB841, 986, 316 }, + { 0x9E19DB92B4E31BA9, 1013, 324 }, + }; + + // This computation gives exactly the same results for k as + // k = ceil((kAlpha - e - 1) * 0.30102999566398114) + // for |e| <= 1500, but doesn't require floating-point operations. + // NB: log_10(2) ~= 78913 / 2^18 + assert(e >= -1500); + assert(e <= 1500); + const int f = kAlpha - e - 1; + const int k = (f * 78913) / (1 << 18) + static_cast<int>(f > 0); + + const int index = (-kCachedPowersMinDecExp + k + (kCachedPowersDecStep - 1)) / kCachedPowersDecStep; + assert(index >= 0); + assert(index < kCachedPowersSize); + static_cast<void>(kCachedPowersSize); // Fix warning. + + const cached_power cached = kCachedPowers[index]; + assert(kAlpha <= cached.e + e + 64); + assert(kGamma >= cached.e + e + 64); + + return cached; +} + +/*! +For n != 0, returns k, such that pow10 := 10^(k-1) <= n < 10^k. +For n == 0, returns 1 and sets pow10 := 1. +*/ +inline int find_largest_pow10(const uint32_t n, uint32_t& pow10) +{ + // LCOV_EXCL_START + if (n >= 1000000000) + { + pow10 = 1000000000; + return 10; + } + // LCOV_EXCL_STOP + else if (n >= 100000000) + { + pow10 = 100000000; + return 9; + } + else if (n >= 10000000) + { + pow10 = 10000000; + return 8; + } + else if (n >= 1000000) + { + pow10 = 1000000; + return 7; + } + else if (n >= 100000) + { + pow10 = 100000; + return 6; + } + else if (n >= 10000) + { + pow10 = 10000; + return 5; + } + else if (n >= 1000) + { + pow10 = 1000; + return 4; + } + else if (n >= 100) + { + pow10 = 100; + return 3; + } + else if (n >= 10) + { + pow10 = 10; + return 2; + } + else + { + pow10 = 1; + return 1; + } +} + +inline void grisu2_round(char* buf, int len, uint64_t dist, uint64_t delta, + uint64_t rest, uint64_t ten_k) +{ + assert(len >= 1); + assert(dist <= delta); + assert(rest <= delta); + assert(ten_k > 0); + + // <--------------------------- delta ----> + // <---- dist ---------> + // --------------[------------------+-------------------]-------------- + // M- w M+ + // + // ten_k + // <------> + // <---- rest ----> + // --------------[------------------+----+--------------]-------------- + // w V + // = buf * 10^k + // + // ten_k represents a unit-in-the-last-place in the decimal representation + // stored in buf. + // Decrement buf by ten_k while this takes buf closer to w. + + // The tests are written in this order to avoid overflow in unsigned + // integer arithmetic. + + while (rest < dist + and delta - rest >= ten_k + and (rest + ten_k < dist or dist - rest > rest + ten_k - dist)) + { + assert(buf[len - 1] != '0'); + buf[len - 1]--; + rest += ten_k; + } +} + +/*! +Generates V = buffer * 10^decimal_exponent, such that M- <= V <= M+. +M- and M+ must be normalized and share the same exponent -60 <= e <= -32. +*/ +inline void grisu2_digit_gen(char* buffer, int& length, int& decimal_exponent, + diyfp M_minus, diyfp w, diyfp M_plus) +{ + static_assert(kAlpha >= -60, "internal error"); + static_assert(kGamma <= -32, "internal error"); + + // Generates the digits (and the exponent) of a decimal floating-point + // number V = buffer * 10^decimal_exponent in the range [M-, M+]. The diyfp's + // w, M- and M+ share the same exponent e, which satisfies alpha <= e <= gamma. + // + // <--------------------------- delta ----> + // <---- dist ---------> + // --------------[------------------+-------------------]-------------- + // M- w M+ + // + // Grisu2 generates the digits of M+ from left to right and stops as soon as + // V is in [M-,M+]. + + assert(M_plus.e >= kAlpha); + assert(M_plus.e <= kGamma); + + uint64_t delta = diyfp::sub(M_plus, M_minus).f; // (significand of (M+ - M-), implicit exponent is e) + uint64_t dist = diyfp::sub(M_plus, w ).f; // (significand of (M+ - w ), implicit exponent is e) + + // Split M+ = f * 2^e into two parts p1 and p2 (note: e < 0): + // + // M+ = f * 2^e + // = ((f div 2^-e) * 2^-e + (f mod 2^-e)) * 2^e + // = ((p1 ) * 2^-e + (p2 )) * 2^e + // = p1 + p2 * 2^e + + const diyfp one(uint64_t{1} << -M_plus.e, M_plus.e); + + auto p1 = static_cast<uint32_t>(M_plus.f >> -one.e); // p1 = f div 2^-e (Since -e >= 32, p1 fits into a 32-bit int.) + uint64_t p2 = M_plus.f & (one.f - 1); // p2 = f mod 2^-e + + // 1) + // + // Generate the digits of the integral part p1 = d[n-1]...d[1]d[0] + + assert(p1 > 0); + + uint32_t pow10; + const int k = find_largest_pow10(p1, pow10); + + // 10^(k-1) <= p1 < 10^k, pow10 = 10^(k-1) + // + // p1 = (p1 div 10^(k-1)) * 10^(k-1) + (p1 mod 10^(k-1)) + // = (d[k-1] ) * 10^(k-1) + (p1 mod 10^(k-1)) + // + // M+ = p1 + p2 * 2^e + // = d[k-1] * 10^(k-1) + (p1 mod 10^(k-1)) + p2 * 2^e + // = d[k-1] * 10^(k-1) + ((p1 mod 10^(k-1)) * 2^-e + p2) * 2^e + // = d[k-1] * 10^(k-1) + ( rest) * 2^e + // + // Now generate the digits d[n] of p1 from left to right (n = k-1,...,0) + // + // p1 = d[k-1]...d[n] * 10^n + d[n-1]...d[0] + // + // but stop as soon as + // + // rest * 2^e = (d[n-1]...d[0] * 2^-e + p2) * 2^e <= delta * 2^e + + int n = k; + while (n > 0) + { + // Invariants: + // M+ = buffer * 10^n + (p1 + p2 * 2^e) (buffer = 0 for n = k) + // pow10 = 10^(n-1) <= p1 < 10^n + // + const uint32_t d = p1 / pow10; // d = p1 div 10^(n-1) + const uint32_t r = p1 % pow10; // r = p1 mod 10^(n-1) + // + // M+ = buffer * 10^n + (d * 10^(n-1) + r) + p2 * 2^e + // = (buffer * 10 + d) * 10^(n-1) + (r + p2 * 2^e) + // + assert(d <= 9); + buffer[length++] = static_cast<char>('0' + d); // buffer := buffer * 10 + d + // + // M+ = buffer * 10^(n-1) + (r + p2 * 2^e) + // + p1 = r; + n--; + // + // M+ = buffer * 10^n + (p1 + p2 * 2^e) + // pow10 = 10^n + // + + // Now check if enough digits have been generated. + // Compute + // + // p1 + p2 * 2^e = (p1 * 2^-e + p2) * 2^e = rest * 2^e + // + // Note: + // Since rest and delta share the same exponent e, it suffices to + // compare the significands. + const uint64_t rest = (uint64_t{p1} << -one.e) + p2; + if (rest <= delta) + { + // V = buffer * 10^n, with M- <= V <= M+. + + decimal_exponent += n; + + // We may now just stop. But instead look if the buffer could be + // decremented to bring V closer to w. + // + // pow10 = 10^n is now 1 ulp in the decimal representation V. + // The rounding procedure works with diyfp's with an implicit + // exponent of e. + // + // 10^n = (10^n * 2^-e) * 2^e = ulp * 2^e + // + const uint64_t ten_n = uint64_t{pow10} << -one.e; + grisu2_round(buffer, length, dist, delta, rest, ten_n); + + return; + } + + pow10 /= 10; + // + // pow10 = 10^(n-1) <= p1 < 10^n + // Invariants restored. + } + + // 2) + // + // The digits of the integral part have been generated: + // + // M+ = d[k-1]...d[1]d[0] + p2 * 2^e + // = buffer + p2 * 2^e + // + // Now generate the digits of the fractional part p2 * 2^e. + // + // Note: + // No decimal point is generated: the exponent is adjusted instead. + // + // p2 actually represents the fraction + // + // p2 * 2^e + // = p2 / 2^-e + // = d[-1] / 10^1 + d[-2] / 10^2 + ... + // + // Now generate the digits d[-m] of p1 from left to right (m = 1,2,...) + // + // p2 * 2^e = d[-1]d[-2]...d[-m] * 10^-m + // + 10^-m * (d[-m-1] / 10^1 + d[-m-2] / 10^2 + ...) + // + // using + // + // 10^m * p2 = ((10^m * p2) div 2^-e) * 2^-e + ((10^m * p2) mod 2^-e) + // = ( d) * 2^-e + ( r) + // + // or + // 10^m * p2 * 2^e = d + r * 2^e + // + // i.e. + // + // M+ = buffer + p2 * 2^e + // = buffer + 10^-m * (d + r * 2^e) + // = (buffer * 10^m + d) * 10^-m + 10^-m * r * 2^e + // + // and stop as soon as 10^-m * r * 2^e <= delta * 2^e + + assert(p2 > delta); + + int m = 0; + for (;;) + { + // Invariant: + // M+ = buffer * 10^-m + 10^-m * (d[-m-1] / 10 + d[-m-2] / 10^2 + ...) * 2^e + // = buffer * 10^-m + 10^-m * (p2 ) * 2^e + // = buffer * 10^-m + 10^-m * (1/10 * (10 * p2) ) * 2^e + // = buffer * 10^-m + 10^-m * (1/10 * ((10*p2 div 2^-e) * 2^-e + (10*p2 mod 2^-e)) * 2^e + // + assert(p2 <= UINT64_MAX / 10); + p2 *= 10; + const uint64_t d = p2 >> -one.e; // d = (10 * p2) div 2^-e + const uint64_t r = p2 & (one.f - 1); // r = (10 * p2) mod 2^-e + // + // M+ = buffer * 10^-m + 10^-m * (1/10 * (d * 2^-e + r) * 2^e + // = buffer * 10^-m + 10^-m * (1/10 * (d + r * 2^e)) + // = (buffer * 10 + d) * 10^(-m-1) + 10^(-m-1) * r * 2^e + // + assert(d <= 9); + buffer[length++] = static_cast<char>('0' + d); // buffer := buffer * 10 + d + // + // M+ = buffer * 10^(-m-1) + 10^(-m-1) * r * 2^e + // + p2 = r; + m++; + // + // M+ = buffer * 10^-m + 10^-m * p2 * 2^e + // Invariant restored. + + // Check if enough digits have been generated. + // + // 10^-m * p2 * 2^e <= delta * 2^e + // p2 * 2^e <= 10^m * delta * 2^e + // p2 <= 10^m * delta + delta *= 10; + dist *= 10; + if (p2 <= delta) + { + break; + } + } + + // V = buffer * 10^-m, with M- <= V <= M+. + + decimal_exponent -= m; + + // 1 ulp in the decimal representation is now 10^-m. + // Since delta and dist are now scaled by 10^m, we need to do the + // same with ulp in order to keep the units in sync. + // + // 10^m * 10^-m = 1 = 2^-e * 2^e = ten_m * 2^e + // + const uint64_t ten_m = one.f; + grisu2_round(buffer, length, dist, delta, p2, ten_m); + + // By construction this algorithm generates the shortest possible decimal + // number (Loitsch, Theorem 6.2) which rounds back to w. + // For an input number of precision p, at least + // + // N = 1 + ceil(p * log_10(2)) + // + // decimal digits are sufficient to identify all binary floating-point + // numbers (Matula, "In-and-Out conversions"). + // This implies that the algorithm does not produce more than N decimal + // digits. + // + // N = 17 for p = 53 (IEEE double precision) + // N = 9 for p = 24 (IEEE single precision) +} + +/*! +v = buf * 10^decimal_exponent +len is the length of the buffer (number of decimal digits) +The buffer must be large enough, i.e. >= max_digits10. +*/ +inline void grisu2(char* buf, int& len, int& decimal_exponent, + diyfp m_minus, diyfp v, diyfp m_plus) +{ + assert(m_plus.e == m_minus.e); + assert(m_plus.e == v.e); + + // --------(-----------------------+-----------------------)-------- (A) + // m- v m+ + // + // --------------------(-----------+-----------------------)-------- (B) + // m- v m+ + // + // First scale v (and m- and m+) such that the exponent is in the range + // [alpha, gamma]. + + const cached_power cached = get_cached_power_for_binary_exponent(m_plus.e); + + const diyfp c_minus_k(cached.f, cached.e); // = c ~= 10^-k + + // The exponent of the products is = v.e + c_minus_k.e + q and is in the range [alpha,gamma] + const diyfp w = diyfp::mul(v, c_minus_k); + const diyfp w_minus = diyfp::mul(m_minus, c_minus_k); + const diyfp w_plus = diyfp::mul(m_plus, c_minus_k); + + // ----(---+---)---------------(---+---)---------------(---+---)---- + // w- w w+ + // = c*m- = c*v = c*m+ + // + // diyfp::mul rounds its result and c_minus_k is approximated too. w, w- and + // w+ are now off by a small amount. + // In fact: + // + // w - v * 10^k < 1 ulp + // + // To account for this inaccuracy, add resp. subtract 1 ulp. + // + // --------+---[---------------(---+---)---------------]---+-------- + // w- M- w M+ w+ + // + // Now any number in [M-, M+] (bounds included) will round to w when input, + // regardless of how the input rounding algorithm breaks ties. + // + // And digit_gen generates the shortest possible such number in [M-, M+]. + // Note that this does not mean that Grisu2 always generates the shortest + // possible number in the interval (m-, m+). + const diyfp M_minus(w_minus.f + 1, w_minus.e); + const diyfp M_plus (w_plus.f - 1, w_plus.e ); + + decimal_exponent = -cached.k; // = -(-k) = k + + grisu2_digit_gen(buf, len, decimal_exponent, M_minus, w, M_plus); +} + +/*! +v = buf * 10^decimal_exponent +len is the length of the buffer (number of decimal digits) +The buffer must be large enough, i.e. >= max_digits10. +*/ +template <typename FloatType> +void grisu2(char* buf, int& len, int& decimal_exponent, FloatType value) +{ + static_assert(diyfp::kPrecision >= std::numeric_limits<FloatType>::digits + 3, + "internal error: not enough precision"); + + assert(std::isfinite(value)); + assert(value > 0); + + // If the neighbors (and boundaries) of 'value' are always computed for double-precision + // numbers, all float's can be recovered using strtod (and strtof). However, the resulting + // decimal representations are not exactly "short". + // + // The documentation for 'std::to_chars' (https://en.cppreference.com/w/cpp/utility/to_chars) + // says "value is converted to a string as if by std::sprintf in the default ("C") locale" + // and since sprintf promotes float's to double's, I think this is exactly what 'std::to_chars' + // does. + // On the other hand, the documentation for 'std::to_chars' requires that "parsing the + // representation using the corresponding std::from_chars function recovers value exactly". That + // indicates that single precision floating-point numbers should be recovered using + // 'std::strtof'. + // + // NB: If the neighbors are computed for single-precision numbers, there is a single float + // (7.0385307e-26f) which can't be recovered using strtod. The resulting double precision + // value is off by 1 ulp. +#if 0 + const boundaries w = compute_boundaries(static_cast<double>(value)); +#else + const boundaries w = compute_boundaries(value); +#endif + + grisu2(buf, len, decimal_exponent, w.minus, w.w, w.plus); +} + +/*! +@brief appends a decimal representation of e to buf +@return a pointer to the element following the exponent. +@pre -1000 < e < 1000 +*/ +inline char* append_exponent(char* buf, int e) +{ + assert(e > -1000); + assert(e < 1000); + + if (e < 0) + { + e = -e; + *buf++ = '-'; + } + else + { + *buf++ = '+'; + } + + auto k = static_cast<uint32_t>(e); + if (k < 10) + { + // Always print at least two digits in the exponent. + // This is for compatibility with printf("%g"). + *buf++ = '0'; + *buf++ = static_cast<char>('0' + k); + } + else if (k < 100) + { + *buf++ = static_cast<char>('0' + k / 10); + k %= 10; + *buf++ = static_cast<char>('0' + k); + } + else + { + *buf++ = static_cast<char>('0' + k / 100); + k %= 100; + *buf++ = static_cast<char>('0' + k / 10); + k %= 10; + *buf++ = static_cast<char>('0' + k); + } + + return buf; +} + +/*! +@brief prettify v = buf * 10^decimal_exponent + +If v is in the range [10^min_exp, 10^max_exp) it will be printed in fixed-point +notation. Otherwise it will be printed in exponential notation. + +@pre min_exp < 0 +@pre max_exp > 0 +*/ +inline char* format_buffer(char* buf, int len, int decimal_exponent, + int min_exp, int max_exp) +{ + assert(min_exp < 0); + assert(max_exp > 0); + + const int k = len; + const int n = len + decimal_exponent; + + // v = buf * 10^(n-k) + // k is the length of the buffer (number of decimal digits) + // n is the position of the decimal point relative to the start of the buffer. + + if (k <= n and n <= max_exp) + { + // digits[000] + // len <= max_exp + 2 + + std::memset(buf + k, '0', static_cast<size_t>(n - k)); + // Make it look like a floating-point number (#362, #378) + buf[n + 0] = '.'; + buf[n + 1] = '0'; + return buf + (n + 2); + } + + if (0 < n and n <= max_exp) + { + // dig.its + // len <= max_digits10 + 1 + + assert(k > n); + + std::memmove(buf + (n + 1), buf + n, static_cast<size_t>(k - n)); + buf[n] = '.'; + return buf + (k + 1); + } + + if (min_exp < n and n <= 0) + { + // 0.[000]digits + // len <= 2 + (-min_exp - 1) + max_digits10 + + std::memmove(buf + (2 + -n), buf, static_cast<size_t>(k)); + buf[0] = '0'; + buf[1] = '.'; + std::memset(buf + 2, '0', static_cast<size_t>(-n)); + return buf + (2 + (-n) + k); + } + + if (k == 1) + { + // dE+123 + // len <= 1 + 5 + + buf += 1; + } + else + { + // d.igitsE+123 + // len <= max_digits10 + 1 + 5 + + std::memmove(buf + 2, buf + 1, static_cast<size_t>(k - 1)); + buf[1] = '.'; + buf += 1 + k; + } + + *buf++ = 'e'; + return append_exponent(buf, n - 1); +} + +} // namespace dtoa_impl + +/*! +@brief generates a decimal representation of the floating-point number value in [first, last). + +The format of the resulting decimal representation is similar to printf's %g +format. Returns an iterator pointing past-the-end of the decimal representation. + +@note The input number must be finite, i.e. NaN's and Inf's are not supported. +@note The buffer must be large enough. +@note The result is NOT null-terminated. +*/ +template <typename FloatType> +char* to_chars(char* first, const char* last, FloatType value) +{ + static_cast<void>(last); // maybe unused - fix warning + assert(std::isfinite(value)); + + // Use signbit(value) instead of (value < 0) since signbit works for -0. + if (std::signbit(value)) + { + value = -value; + *first++ = '-'; + } + + if (value == 0) // +-0 + { + *first++ = '0'; + // Make it look like a floating-point number (#362, #378) + *first++ = '.'; + *first++ = '0'; + return first; + } + + assert(last - first >= std::numeric_limits<FloatType>::max_digits10); + + // Compute v = buffer * 10^decimal_exponent. + // The decimal digits are stored in the buffer, which needs to be interpreted + // as an unsigned decimal integer. + // len is the length of the buffer, i.e. the number of decimal digits. + int len = 0; + int decimal_exponent = 0; + dtoa_impl::grisu2(first, len, decimal_exponent, value); + + assert(len <= std::numeric_limits<FloatType>::max_digits10); + + // Format the buffer like printf("%.*g", prec, value) + constexpr int kMinExp = -4; + // Use digits10 here to increase compatibility with version 2. + constexpr int kMaxExp = std::numeric_limits<FloatType>::digits10; + + assert(last - first >= kMaxExp + 2); + assert(last - first >= 2 + (-kMinExp - 1) + std::numeric_limits<FloatType>::max_digits10); + assert(last - first >= std::numeric_limits<FloatType>::max_digits10 + 6); + + return dtoa_impl::format_buffer(first, len, decimal_exponent, kMinExp, kMaxExp); +} + +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/meta/cpp_future.hpp> + +// #include <nlohmann/detail/output/binary_writer.hpp> + +// #include <nlohmann/detail/output/output_adapters.hpp> + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +namespace detail +{ +/////////////////// +// serialization // +/////////////////// + +/// how to treat decoding errors +enum class error_handler_t +{ + strict, ///< throw a type_error exception in case of invalid UTF-8 + replace, ///< replace invalid UTF-8 sequences with U+FFFD + ignore ///< ignore invalid UTF-8 sequences +}; + +template<typename BasicJsonType> +class serializer +{ + using string_t = typename BasicJsonType::string_t; + using number_float_t = typename BasicJsonType::number_float_t; + using number_integer_t = typename BasicJsonType::number_integer_t; + using number_unsigned_t = typename BasicJsonType::number_unsigned_t; + static constexpr uint8_t UTF8_ACCEPT = 0; + static constexpr uint8_t UTF8_REJECT = 1; + + public: + /*! + @param[in] s output stream to serialize to + @param[in] ichar indentation character to use + @param[in] error_handler_ how to react on decoding errors + */ + serializer(output_adapter_t<char> s, const char ichar, + error_handler_t error_handler_ = error_handler_t::strict) + : o(std::move(s)) + , loc(std::localeconv()) + , thousands_sep(loc->thousands_sep == nullptr ? '\0' : * (loc->thousands_sep)) + , decimal_point(loc->decimal_point == nullptr ? '\0' : * (loc->decimal_point)) + , indent_char(ichar) + , indent_string(512, indent_char) + , error_handler(error_handler_) + {} + + // delete because of pointer members + serializer(const serializer&) = delete; + serializer& operator=(const serializer&) = delete; + serializer(serializer&&) = delete; + serializer& operator=(serializer&&) = delete; + ~serializer() = default; + + /*! + @brief internal implementation of the serialization function + + This function is called by the public member function dump and organizes + the serialization internally. The indentation level is propagated as + additional parameter. In case of arrays and objects, the function is + called recursively. + + - strings and object keys are escaped using `escape_string()` + - integer numbers are converted implicitly via `operator<<` + - floating-point numbers are converted to a string using `"%g"` format + + @param[in] val value to serialize + @param[in] pretty_print whether the output shall be pretty-printed + @param[in] indent_step the indent level + @param[in] current_indent the current indent level (only used internally) + */ + void dump(const BasicJsonType& val, const bool pretty_print, + const bool ensure_ascii, + const unsigned int indent_step, + const unsigned int current_indent = 0) + { + switch (val.m_type) + { + case value_t::object: + { + if (val.m_value.object->empty()) + { + o->write_characters("{}", 2); + return; + } + + if (pretty_print) + { + o->write_characters("{\n", 2); + + // variable to hold indentation for recursive calls + const auto new_indent = current_indent + indent_step; + if (JSON_UNLIKELY(indent_string.size() < new_indent)) + { + indent_string.resize(indent_string.size() * 2, ' '); + } + + // first n-1 elements + auto i = val.m_value.object->cbegin(); + for (std::size_t cnt = 0; cnt < val.m_value.object->size() - 1; ++cnt, ++i) + { + o->write_characters(indent_string.c_str(), new_indent); + o->write_character('\"'); + dump_escaped(i->first, ensure_ascii); + o->write_characters("\": ", 3); + dump(i->second, true, ensure_ascii, indent_step, new_indent); + o->write_characters(",\n", 2); + } + + // last element + assert(i != val.m_value.object->cend()); + assert(std::next(i) == val.m_value.object->cend()); + o->write_characters(indent_string.c_str(), new_indent); + o->write_character('\"'); + dump_escaped(i->first, ensure_ascii); + o->write_characters("\": ", 3); + dump(i->second, true, ensure_ascii, indent_step, new_indent); + + o->write_character('\n'); + o->write_characters(indent_string.c_str(), current_indent); + o->write_character('}'); + } + else + { + o->write_character('{'); + + // first n-1 elements + auto i = val.m_value.object->cbegin(); + for (std::size_t cnt = 0; cnt < val.m_value.object->size() - 1; ++cnt, ++i) + { + o->write_character('\"'); + dump_escaped(i->first, ensure_ascii); + o->write_characters("\":", 2); + dump(i->second, false, ensure_ascii, indent_step, current_indent); + o->write_character(','); + } + + // last element + assert(i != val.m_value.object->cend()); + assert(std::next(i) == val.m_value.object->cend()); + o->write_character('\"'); + dump_escaped(i->first, ensure_ascii); + o->write_characters("\":", 2); + dump(i->second, false, ensure_ascii, indent_step, current_indent); + + o->write_character('}'); + } + + return; + } + + case value_t::array: + { + if (val.m_value.array->empty()) + { + o->write_characters("[]", 2); + return; + } + + if (pretty_print) + { + o->write_characters("[\n", 2); + + // variable to hold indentation for recursive calls + const auto new_indent = current_indent + indent_step; + if (JSON_UNLIKELY(indent_string.size() < new_indent)) + { + indent_string.resize(indent_string.size() * 2, ' '); + } + + // first n-1 elements + for (auto i = val.m_value.array->cbegin(); + i != val.m_value.array->cend() - 1; ++i) + { + o->write_characters(indent_string.c_str(), new_indent); + dump(*i, true, ensure_ascii, indent_step, new_indent); + o->write_characters(",\n", 2); + } + + // last element + assert(not val.m_value.array->empty()); + o->write_characters(indent_string.c_str(), new_indent); + dump(val.m_value.array->back(), true, ensure_ascii, indent_step, new_indent); + + o->write_character('\n'); + o->write_characters(indent_string.c_str(), current_indent); + o->write_character(']'); + } + else + { + o->write_character('['); + + // first n-1 elements + for (auto i = val.m_value.array->cbegin(); + i != val.m_value.array->cend() - 1; ++i) + { + dump(*i, false, ensure_ascii, indent_step, current_indent); + o->write_character(','); + } + + // last element + assert(not val.m_value.array->empty()); + dump(val.m_value.array->back(), false, ensure_ascii, indent_step, current_indent); + + o->write_character(']'); + } + + return; + } + + case value_t::string: + { + o->write_character('\"'); + dump_escaped(*val.m_value.string, ensure_ascii); + o->write_character('\"'); + return; + } + + case value_t::boolean: + { + if (val.m_value.boolean) + { + o->write_characters("true", 4); + } + else + { + o->write_characters("false", 5); + } + return; + } + + case value_t::number_integer: + { + dump_integer(val.m_value.number_integer); + return; + } + + case value_t::number_unsigned: + { + dump_integer(val.m_value.number_unsigned); + return; + } + + case value_t::number_float: + { + dump_float(val.m_value.number_float); + return; + } + + case value_t::discarded: + { + o->write_characters("<discarded>", 11); + return; + } + + case value_t::null: + { + o->write_characters("null", 4); + return; + } + } + } + + private: + /*! + @brief dump escaped string + + Escape a string by replacing certain special characters by a sequence of an + escape character (backslash) and another character and other control + characters by a sequence of "\u" followed by a four-digit hex + representation. The escaped string is written to output stream @a o. + + @param[in] s the string to escape + @param[in] ensure_ascii whether to escape non-ASCII characters with + \uXXXX sequences + + @complexity Linear in the length of string @a s. + */ + void dump_escaped(const string_t& s, const bool ensure_ascii) + { + uint32_t codepoint; + uint8_t state = UTF8_ACCEPT; + std::size_t bytes = 0; // number of bytes written to string_buffer + + // number of bytes written at the point of the last valid byte + std::size_t bytes_after_last_accept = 0; + std::size_t undumped_chars = 0; + + for (std::size_t i = 0; i < s.size(); ++i) + { + const auto byte = static_cast<uint8_t>(s[i]); + + switch (decode(state, codepoint, byte)) + { + case UTF8_ACCEPT: // decode found a new code point + { + switch (codepoint) + { + case 0x08: // backspace + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = 'b'; + break; + } + + case 0x09: // horizontal tab + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = 't'; + break; + } + + case 0x0A: // newline + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = 'n'; + break; + } + + case 0x0C: // formfeed + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = 'f'; + break; + } + + case 0x0D: // carriage return + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = 'r'; + break; + } + + case 0x22: // quotation mark + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = '\"'; + break; + } + + case 0x5C: // reverse solidus + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = '\\'; + break; + } + + default: + { + // escape control characters (0x00..0x1F) or, if + // ensure_ascii parameter is used, non-ASCII characters + if ((codepoint <= 0x1F) or (ensure_ascii and (codepoint >= 0x7F))) + { + if (codepoint <= 0xFFFF) + { + std::snprintf(string_buffer.data() + bytes, 7, "\\u%04x", + static_cast<uint16_t>(codepoint)); + bytes += 6; + } + else + { + std::snprintf(string_buffer.data() + bytes, 13, "\\u%04x\\u%04x", + static_cast<uint16_t>(0xD7C0 + (codepoint >> 10)), + static_cast<uint16_t>(0xDC00 + (codepoint & 0x3FF))); + bytes += 12; + } + } + else + { + // copy byte to buffer (all previous bytes + // been copied have in default case above) + string_buffer[bytes++] = s[i]; + } + break; + } + } + + // write buffer and reset index; there must be 13 bytes + // left, as this is the maximal number of bytes to be + // written ("\uxxxx\uxxxx\0") for one code point + if (string_buffer.size() - bytes < 13) + { + o->write_characters(string_buffer.data(), bytes); + bytes = 0; + } + + // remember the byte position of this accept + bytes_after_last_accept = bytes; + undumped_chars = 0; + break; + } + + case UTF8_REJECT: // decode found invalid UTF-8 byte + { + switch (error_handler) + { + case error_handler_t::strict: + { + std::string sn(3, '\0'); + snprintf(&sn[0], sn.size(), "%.2X", byte); + JSON_THROW(type_error::create(316, "invalid UTF-8 byte at index " + std::to_string(i) + ": 0x" + sn)); + } + + case error_handler_t::ignore: + case error_handler_t::replace: + { + // in case we saw this character the first time, we + // would like to read it again, because the byte + // may be OK for itself, but just not OK for the + // previous sequence + if (undumped_chars > 0) + { + --i; + } + + // reset length buffer to the last accepted index; + // thus removing/ignoring the invalid characters + bytes = bytes_after_last_accept; + + if (error_handler == error_handler_t::replace) + { + // add a replacement character + if (ensure_ascii) + { + string_buffer[bytes++] = '\\'; + string_buffer[bytes++] = 'u'; + string_buffer[bytes++] = 'f'; + string_buffer[bytes++] = 'f'; + string_buffer[bytes++] = 'f'; + string_buffer[bytes++] = 'd'; + } + else + { + string_buffer[bytes++] = detail::binary_writer<BasicJsonType, char>::to_char_type('\xEF'); + string_buffer[bytes++] = detail::binary_writer<BasicJsonType, char>::to_char_type('\xBF'); + string_buffer[bytes++] = detail::binary_writer<BasicJsonType, char>::to_char_type('\xBD'); + } + bytes_after_last_accept = bytes; + } + + undumped_chars = 0; + + // continue processing the string + state = UTF8_ACCEPT; + break; + } + } + break; + } + + default: // decode found yet incomplete multi-byte code point + { + if (not ensure_ascii) + { + // code point will not be escaped - copy byte to buffer + string_buffer[bytes++] = s[i]; + } + ++undumped_chars; + break; + } + } + } + + // we finished processing the string + if (JSON_LIKELY(state == UTF8_ACCEPT)) + { + // write buffer + if (bytes > 0) + { + o->write_characters(string_buffer.data(), bytes); + } + } + else + { + // we finish reading, but do not accept: string was incomplete + switch (error_handler) + { + case error_handler_t::strict: + { + std::string sn(3, '\0'); + snprintf(&sn[0], sn.size(), "%.2X", static_cast<uint8_t>(s.back())); + JSON_THROW(type_error::create(316, "incomplete UTF-8 string; last byte: 0x" + sn)); + } + + case error_handler_t::ignore: + { + // write all accepted bytes + o->write_characters(string_buffer.data(), bytes_after_last_accept); + break; + } + + case error_handler_t::replace: + { + // write all accepted bytes + o->write_characters(string_buffer.data(), bytes_after_last_accept); + // add a replacement character + if (ensure_ascii) + { + o->write_characters("\\ufffd", 6); + } + else + { + o->write_characters("\xEF\xBF\xBD", 3); + } + break; + } + } + } + } + + /*! + @brief dump an integer + + Dump a given integer to output stream @a o. Works internally with + @a number_buffer. + + @param[in] x integer number (signed or unsigned) to dump + @tparam NumberType either @a number_integer_t or @a number_unsigned_t + */ + template<typename NumberType, detail::enable_if_t< + std::is_same<NumberType, number_unsigned_t>::value or + std::is_same<NumberType, number_integer_t>::value, + int> = 0> + void dump_integer(NumberType x) + { + // special case for "0" + if (x == 0) + { + o->write_character('0'); + return; + } + + const bool is_negative = std::is_same<NumberType, number_integer_t>::value and not (x >= 0); // see issue #755 + std::size_t i = 0; + + while (x != 0) + { + // spare 1 byte for '\0' + assert(i < number_buffer.size() - 1); + + const auto digit = std::labs(static_cast<long>(x % 10)); + number_buffer[i++] = static_cast<char>('0' + digit); + x /= 10; + } + + if (is_negative) + { + // make sure there is capacity for the '-' + assert(i < number_buffer.size() - 2); + number_buffer[i++] = '-'; + } + + std::reverse(number_buffer.begin(), number_buffer.begin() + i); + o->write_characters(number_buffer.data(), i); + } + + /*! + @brief dump a floating-point number + + Dump a given floating-point number to output stream @a o. Works internally + with @a number_buffer. + + @param[in] x floating-point number to dump + */ + void dump_float(number_float_t x) + { + // NaN / inf + if (not std::isfinite(x)) + { + o->write_characters("null", 4); + return; + } + + // If number_float_t is an IEEE-754 single or double precision number, + // use the Grisu2 algorithm to produce short numbers which are + // guaranteed to round-trip, using strtof and strtod, resp. + // + // NB: The test below works if <long double> == <double>. + static constexpr bool is_ieee_single_or_double + = (std::numeric_limits<number_float_t>::is_iec559 and std::numeric_limits<number_float_t>::digits == 24 and std::numeric_limits<number_float_t>::max_exponent == 128) or + (std::numeric_limits<number_float_t>::is_iec559 and std::numeric_limits<number_float_t>::digits == 53 and std::numeric_limits<number_float_t>::max_exponent == 1024); + + dump_float(x, std::integral_constant<bool, is_ieee_single_or_double>()); + } + + void dump_float(number_float_t x, std::true_type /*is_ieee_single_or_double*/) + { + char* begin = number_buffer.data(); + char* end = ::nlohmann::detail::to_chars(begin, begin + number_buffer.size(), x); + + o->write_characters(begin, static_cast<size_t>(end - begin)); + } + + void dump_float(number_float_t x, std::false_type /*is_ieee_single_or_double*/) + { + // get number of digits for a float -> text -> float round-trip + static constexpr auto d = std::numeric_limits<number_float_t>::max_digits10; + + // the actual conversion + std::ptrdiff_t len = snprintf(number_buffer.data(), number_buffer.size(), "%.*g", d, x); + + // negative value indicates an error + assert(len > 0); + // check if buffer was large enough + assert(static_cast<std::size_t>(len) < number_buffer.size()); + + // erase thousands separator + if (thousands_sep != '\0') + { + const auto end = std::remove(number_buffer.begin(), + number_buffer.begin() + len, thousands_sep); + std::fill(end, number_buffer.end(), '\0'); + assert((end - number_buffer.begin()) <= len); + len = (end - number_buffer.begin()); + } + + // convert decimal point to '.' + if (decimal_point != '\0' and decimal_point != '.') + { + const auto dec_pos = std::find(number_buffer.begin(), number_buffer.end(), decimal_point); + if (dec_pos != number_buffer.end()) + { + *dec_pos = '.'; + } + } + + o->write_characters(number_buffer.data(), static_cast<std::size_t>(len)); + + // determine if need to append ".0" + const bool value_is_int_like = + std::none_of(number_buffer.begin(), number_buffer.begin() + len + 1, + [](char c) + { + return (c == '.' or c == 'e'); + }); + + if (value_is_int_like) + { + o->write_characters(".0", 2); + } + } + + /*! + @brief check whether a string is UTF-8 encoded + + The function checks each byte of a string whether it is UTF-8 encoded. The + result of the check is stored in the @a state parameter. The function must + be called initially with state 0 (accept). State 1 means the string must + be rejected, because the current byte is not allowed. If the string is + completely processed, but the state is non-zero, the string ended + prematurely; that is, the last byte indicated more bytes should have + followed. + + @param[in,out] state the state of the decoding + @param[in,out] codep codepoint (valid only if resulting state is UTF8_ACCEPT) + @param[in] byte next byte to decode + @return new state + + @note The function has been edited: a std::array is used. + + @copyright Copyright (c) 2008-2009 Bjoern Hoehrmann <bjoern@hoehrmann.de> + @sa http://bjoern.hoehrmann.de/utf-8/decoder/dfa/ + */ + static uint8_t decode(uint8_t& state, uint32_t& codep, const uint8_t byte) noexcept + { + static const std::array<uint8_t, 400> utf8d = + { + { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 00..1F + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 20..3F + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 40..5F + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 60..7F + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, // 80..9F + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, // A0..BF + 8, 8, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // C0..DF + 0xA, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x3, 0x4, 0x3, 0x3, // E0..EF + 0xB, 0x6, 0x6, 0x6, 0x5, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, 0x8, // F0..FF + 0x0, 0x1, 0x2, 0x3, 0x5, 0x8, 0x7, 0x1, 0x1, 0x1, 0x4, 0x6, 0x1, 0x1, 0x1, 0x1, // s0..s0 + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, // s1..s2 + 1, 2, 1, 1, 1, 1, 1, 2, 1, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 1, 1, 1, // s3..s4 + 1, 2, 1, 1, 1, 1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3, 1, 3, 1, 1, 1, 1, 1, 1, // s5..s6 + 1, 3, 1, 1, 1, 1, 1, 3, 1, 3, 1, 1, 1, 1, 1, 1, 1, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // s7..s8 + } + }; + + const uint8_t type = utf8d[byte]; + + codep = (state != UTF8_ACCEPT) + ? (byte & 0x3fu) | (codep << 6) + : static_cast<uint32_t>(0xff >> type) & (byte); + + state = utf8d[256u + state * 16u + type]; + return state; + } + + private: + /// the output of the serializer + output_adapter_t<char> o = nullptr; + + /// a (hopefully) large enough character buffer + std::array<char, 64> number_buffer{{}}; + + /// the locale + const std::lconv* loc = nullptr; + /// the locale's thousand separator character + const char thousands_sep = '\0'; + /// the locale's decimal point character + const char decimal_point = '\0'; + + /// string buffer + std::array<char, 512> string_buffer{{}}; + + /// the indentation character + const char indent_char; + /// the indentation string + string_t indent_string; + + /// error_handler how to react on decoding errors + const error_handler_t error_handler; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/json_ref.hpp> + + +#include <initializer_list> +#include <utility> + +// #include <nlohmann/detail/meta/type_traits.hpp> + + +namespace nlohmann +{ +namespace detail +{ +template<typename BasicJsonType> +class json_ref +{ + public: + using value_type = BasicJsonType; + + json_ref(value_type&& value) + : owned_value(std::move(value)), value_ref(&owned_value), is_rvalue(true) + {} + + json_ref(const value_type& value) + : value_ref(const_cast<value_type*>(&value)), is_rvalue(false) + {} + + json_ref(std::initializer_list<json_ref> init) + : owned_value(init), value_ref(&owned_value), is_rvalue(true) + {} + + template < + class... Args, + enable_if_t<std::is_constructible<value_type, Args...>::value, int> = 0 > + json_ref(Args && ... args) + : owned_value(std::forward<Args>(args)...), value_ref(&owned_value), + is_rvalue(true) {} + + // class should be movable only + json_ref(json_ref&&) = default; + json_ref(const json_ref&) = delete; + json_ref& operator=(const json_ref&) = delete; + json_ref& operator=(json_ref&&) = delete; + ~json_ref() = default; + + value_type moved_or_copied() const + { + if (is_rvalue) + { + return std::move(*value_ref); + } + return *value_ref; + } + + value_type const& operator*() const + { + return *static_cast<value_type const*>(value_ref); + } + + value_type const* operator->() const + { + return static_cast<value_type const*>(value_ref); + } + + private: + mutable value_type owned_value = nullptr; + value_type* value_ref = nullptr; + const bool is_rvalue; +}; +} // namespace detail +} // namespace nlohmann + +// #include <nlohmann/detail/json_pointer.hpp> + + +#include <cassert> // assert +#include <numeric> // accumulate +#include <string> // string +#include <vector> // vector + +// #include <nlohmann/detail/macro_scope.hpp> + +// #include <nlohmann/detail/exceptions.hpp> + +// #include <nlohmann/detail/value_t.hpp> + + +namespace nlohmann +{ +template<typename BasicJsonType> +class json_pointer +{ + // allow basic_json to access private members + NLOHMANN_BASIC_JSON_TPL_DECLARATION + friend class basic_json; + + public: + /*! + @brief create JSON pointer + + Create a JSON pointer according to the syntax described in + [Section 3 of RFC6901](https://tools.ietf.org/html/rfc6901#section-3). + + @param[in] s string representing the JSON pointer; if omitted, the empty + string is assumed which references the whole JSON value + + @throw parse_error.107 if the given JSON pointer @a s is nonempty and does + not begin with a slash (`/`); see example below + + @throw parse_error.108 if a tilde (`~`) in the given JSON pointer @a s is + not followed by `0` (representing `~`) or `1` (representing `/`); see + example below + + @liveexample{The example shows the construction several valid JSON pointers + as well as the exceptional behavior.,json_pointer} + + @since version 2.0.0 + */ + explicit json_pointer(const std::string& s = "") + : reference_tokens(split(s)) + {} + + /*! + @brief return a string representation of the JSON pointer + + @invariant For each JSON pointer `ptr`, it holds: + @code {.cpp} + ptr == json_pointer(ptr.to_string()); + @endcode + + @return a string representation of the JSON pointer + + @liveexample{The example shows the result of `to_string`., + json_pointer__to_string} + + @since version 2.0.0 + */ + std::string to_string() const + { + return std::accumulate(reference_tokens.begin(), reference_tokens.end(), + std::string{}, + [](const std::string & a, const std::string & b) + { + return a + "/" + escape(b); + }); + } + + /// @copydoc to_string() + operator std::string() const + { + return to_string(); + } + + /*! + @param[in] s reference token to be converted into an array index + + @return integer representation of @a s + + @throw out_of_range.404 if string @a s could not be converted to an integer + */ + static int array_index(const std::string& s) + { + std::size_t processed_chars = 0; + const int res = std::stoi(s, &processed_chars); + + // check if the string was completely read + if (JSON_UNLIKELY(processed_chars != s.size())) + { + JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + s + "'")); + } + + return res; + } + + private: + /*! + @brief remove and return last reference pointer + @throw out_of_range.405 if JSON pointer has no parent + */ + std::string pop_back() + { + if (JSON_UNLIKELY(is_root())) + { + JSON_THROW(detail::out_of_range::create(405, "JSON pointer has no parent")); + } + + auto last = reference_tokens.back(); + reference_tokens.pop_back(); + return last; + } + + /// return whether pointer points to the root document + bool is_root() const noexcept + { + return reference_tokens.empty(); + } + + json_pointer top() const + { + if (JSON_UNLIKELY(is_root())) + { + JSON_THROW(detail::out_of_range::create(405, "JSON pointer has no parent")); + } + + json_pointer result = *this; + result.reference_tokens = {reference_tokens[0]}; + return result; + } + + /*! + @brief create and return a reference to the pointed to value + + @complexity Linear in the number of reference tokens. + + @throw parse_error.109 if array index is not a number + @throw type_error.313 if value cannot be unflattened + */ + BasicJsonType& get_and_create(BasicJsonType& j) const + { + using size_type = typename BasicJsonType::size_type; + auto result = &j; + + // in case no reference tokens exist, return a reference to the JSON value + // j which will be overwritten by a primitive value + for (const auto& reference_token : reference_tokens) + { + switch (result->m_type) + { + case detail::value_t::null: + { + if (reference_token == "0") + { + // start a new array if reference token is 0 + result = &result->operator[](0); + } + else + { + // start a new object otherwise + result = &result->operator[](reference_token); + } + break; + } + + case detail::value_t::object: + { + // create an entry in the object + result = &result->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + // create an entry in the array + JSON_TRY + { + result = &result->operator[](static_cast<size_type>(array_index(reference_token))); + } + JSON_CATCH(std::invalid_argument&) + { + JSON_THROW(detail::parse_error::create(109, 0, "array index '" + reference_token + "' is not a number")); + } + break; + } + + /* + The following code is only reached if there exists a reference + token _and_ the current value is primitive. In this case, we have + an error situation, because primitive values may only occur as + single value; that is, with an empty list of reference tokens. + */ + default: + JSON_THROW(detail::type_error::create(313, "invalid value to unflatten")); + } + } + + return *result; + } + + /*! + @brief return a reference to the pointed to value + + @note This version does not throw if a value is not present, but tries to + create nested values instead. For instance, calling this function + with pointer `"/this/that"` on a null value is equivalent to calling + `operator[]("this").operator[]("that")` on that value, effectively + changing the null value to an object. + + @param[in] ptr a JSON value + + @return reference to the JSON value pointed to by the JSON pointer + + @complexity Linear in the length of the JSON pointer. + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + BasicJsonType& get_unchecked(BasicJsonType* ptr) const + { + using size_type = typename BasicJsonType::size_type; + for (const auto& reference_token : reference_tokens) + { + // convert null values to arrays or objects before continuing + if (ptr->m_type == detail::value_t::null) + { + // check if reference token is a number + const bool nums = + std::all_of(reference_token.begin(), reference_token.end(), + [](const char x) + { + return (x >= '0' and x <= '9'); + }); + + // change value to array for numbers or "-" or to object otherwise + *ptr = (nums or reference_token == "-") + ? detail::value_t::array + : detail::value_t::object; + } + + switch (ptr->m_type) + { + case detail::value_t::object: + { + // use unchecked object access + ptr = &ptr->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + // error condition (cf. RFC 6901, Sect. 4) + if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0')) + { + JSON_THROW(detail::parse_error::create(106, 0, + "array index '" + reference_token + + "' must not begin with '0'")); + } + + if (reference_token == "-") + { + // explicitly treat "-" as index beyond the end + ptr = &ptr->operator[](ptr->m_value.array->size()); + } + else + { + // convert array index to number; unchecked access + JSON_TRY + { + ptr = &ptr->operator[]( + static_cast<size_type>(array_index(reference_token))); + } + JSON_CATCH(std::invalid_argument&) + { + JSON_THROW(detail::parse_error::create(109, 0, "array index '" + reference_token + "' is not a number")); + } + } + break; + } + + default: + JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + reference_token + "'")); + } + } + + return *ptr; + } + + /*! + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + BasicJsonType& get_checked(BasicJsonType* ptr) const + { + using size_type = typename BasicJsonType::size_type; + for (const auto& reference_token : reference_tokens) + { + switch (ptr->m_type) + { + case detail::value_t::object: + { + // note: at performs range check + ptr = &ptr->at(reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_UNLIKELY(reference_token == "-")) + { + // "-" always fails the range check + JSON_THROW(detail::out_of_range::create(402, + "array index '-' (" + std::to_string(ptr->m_value.array->size()) + + ") is out of range")); + } + + // error condition (cf. RFC 6901, Sect. 4) + if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0')) + { + JSON_THROW(detail::parse_error::create(106, 0, + "array index '" + reference_token + + "' must not begin with '0'")); + } + + // note: at performs range check + JSON_TRY + { + ptr = &ptr->at(static_cast<size_type>(array_index(reference_token))); + } + JSON_CATCH(std::invalid_argument&) + { + JSON_THROW(detail::parse_error::create(109, 0, "array index '" + reference_token + "' is not a number")); + } + break; + } + + default: + JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + reference_token + "'")); + } + } + + return *ptr; + } + + /*! + @brief return a const reference to the pointed to value + + @param[in] ptr a JSON value + + @return const reference to the JSON value pointed to by the JSON + pointer + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + const BasicJsonType& get_unchecked(const BasicJsonType* ptr) const + { + using size_type = typename BasicJsonType::size_type; + for (const auto& reference_token : reference_tokens) + { + switch (ptr->m_type) + { + case detail::value_t::object: + { + // use unchecked object access + ptr = &ptr->operator[](reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_UNLIKELY(reference_token == "-")) + { + // "-" cannot be used for const access + JSON_THROW(detail::out_of_range::create(402, + "array index '-' (" + std::to_string(ptr->m_value.array->size()) + + ") is out of range")); + } + + // error condition (cf. RFC 6901, Sect. 4) + if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0')) + { + JSON_THROW(detail::parse_error::create(106, 0, + "array index '" + reference_token + + "' must not begin with '0'")); + } + + // use unchecked array access + JSON_TRY + { + ptr = &ptr->operator[]( + static_cast<size_type>(array_index(reference_token))); + } + JSON_CATCH(std::invalid_argument&) + { + JSON_THROW(detail::parse_error::create(109, 0, "array index '" + reference_token + "' is not a number")); + } + break; + } + + default: + JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + reference_token + "'")); + } + } + + return *ptr; + } + + /*! + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + */ + const BasicJsonType& get_checked(const BasicJsonType* ptr) const + { + using size_type = typename BasicJsonType::size_type; + for (const auto& reference_token : reference_tokens) + { + switch (ptr->m_type) + { + case detail::value_t::object: + { + // note: at performs range check + ptr = &ptr->at(reference_token); + break; + } + + case detail::value_t::array: + { + if (JSON_UNLIKELY(reference_token == "-")) + { + // "-" always fails the range check + JSON_THROW(detail::out_of_range::create(402, + "array index '-' (" + std::to_string(ptr->m_value.array->size()) + + ") is out of range")); + } + + // error condition (cf. RFC 6901, Sect. 4) + if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0')) + { + JSON_THROW(detail::parse_error::create(106, 0, + "array index '" + reference_token + + "' must not begin with '0'")); + } + + // note: at performs range check + JSON_TRY + { + ptr = &ptr->at(static_cast<size_type>(array_index(reference_token))); + } + JSON_CATCH(std::invalid_argument&) + { + JSON_THROW(detail::parse_error::create(109, 0, "array index '" + reference_token + "' is not a number")); + } + break; + } + + default: + JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + reference_token + "'")); + } + } + + return *ptr; + } + + /*! + @brief split the string input to reference tokens + + @note This function is only called by the json_pointer constructor. + All exceptions below are documented there. + + @throw parse_error.107 if the pointer is not empty or begins with '/' + @throw parse_error.108 if character '~' is not followed by '0' or '1' + */ + static std::vector<std::string> split(const std::string& reference_string) + { + std::vector<std::string> result; + + // special case: empty reference string -> no reference tokens + if (reference_string.empty()) + { + return result; + } + + // check if nonempty reference string begins with slash + if (JSON_UNLIKELY(reference_string[0] != '/')) + { + JSON_THROW(detail::parse_error::create(107, 1, + "JSON pointer must be empty or begin with '/' - was: '" + + reference_string + "'")); + } + + // extract the reference tokens: + // - slash: position of the last read slash (or end of string) + // - start: position after the previous slash + for ( + // search for the first slash after the first character + std::size_t slash = reference_string.find_first_of('/', 1), + // set the beginning of the first reference token + start = 1; + // we can stop if start == 0 (if slash == std::string::npos) + start != 0; + // set the beginning of the next reference token + // (will eventually be 0 if slash == std::string::npos) + start = (slash == std::string::npos) ? 0 : slash + 1, + // find next slash + slash = reference_string.find_first_of('/', start)) + { + // use the text between the beginning of the reference token + // (start) and the last slash (slash). + auto reference_token = reference_string.substr(start, slash - start); + + // check reference tokens are properly escaped + for (std::size_t pos = reference_token.find_first_of('~'); + pos != std::string::npos; + pos = reference_token.find_first_of('~', pos + 1)) + { + assert(reference_token[pos] == '~'); + + // ~ must be followed by 0 or 1 + if (JSON_UNLIKELY(pos == reference_token.size() - 1 or + (reference_token[pos + 1] != '0' and + reference_token[pos + 1] != '1'))) + { + JSON_THROW(detail::parse_error::create(108, 0, "escape character '~' must be followed with '0' or '1'")); + } + } + + // finally, store the reference token + unescape(reference_token); + result.push_back(reference_token); + } + + return result; + } + + /*! + @brief replace all occurrences of a substring by another string + + @param[in,out] s the string to manipulate; changed so that all + occurrences of @a f are replaced with @a t + @param[in] f the substring to replace with @a t + @param[in] t the string to replace @a f + + @pre The search string @a f must not be empty. **This precondition is + enforced with an assertion.** + + @since version 2.0.0 + */ + static void replace_substring(std::string& s, const std::string& f, + const std::string& t) + { + assert(not f.empty()); + for (auto pos = s.find(f); // find first occurrence of f + pos != std::string::npos; // make sure f was found + s.replace(pos, f.size(), t), // replace with t, and + pos = s.find(f, pos + t.size())) // find next occurrence of f + {} + } + + /// escape "~" to "~0" and "/" to "~1" + static std::string escape(std::string s) + { + replace_substring(s, "~", "~0"); + replace_substring(s, "/", "~1"); + return s; + } + + /// unescape "~1" to tilde and "~0" to slash (order is important!) + static void unescape(std::string& s) + { + replace_substring(s, "~1", "/"); + replace_substring(s, "~0", "~"); + } + + /*! + @param[in] reference_string the reference string to the current value + @param[in] value the value to consider + @param[in,out] result the result object to insert values to + + @note Empty objects or arrays are flattened to `null`. + */ + static void flatten(const std::string& reference_string, + const BasicJsonType& value, + BasicJsonType& result) + { + switch (value.m_type) + { + case detail::value_t::array: + { + if (value.m_value.array->empty()) + { + // flatten empty array as null + result[reference_string] = nullptr; + } + else + { + // iterate array and use index as reference string + for (std::size_t i = 0; i < value.m_value.array->size(); ++i) + { + flatten(reference_string + "/" + std::to_string(i), + value.m_value.array->operator[](i), result); + } + } + break; + } + + case detail::value_t::object: + { + if (value.m_value.object->empty()) + { + // flatten empty object as null + result[reference_string] = nullptr; + } + else + { + // iterate object and use keys as reference string + for (const auto& element : *value.m_value.object) + { + flatten(reference_string + "/" + escape(element.first), element.second, result); + } + } + break; + } + + default: + { + // add primitive value with its reference string + result[reference_string] = value; + break; + } + } + } + + /*! + @param[in] value flattened JSON + + @return unflattened JSON + + @throw parse_error.109 if array index is not a number + @throw type_error.314 if value is not an object + @throw type_error.315 if object values are not primitive + @throw type_error.313 if value cannot be unflattened + */ + static BasicJsonType + unflatten(const BasicJsonType& value) + { + if (JSON_UNLIKELY(not value.is_object())) + { + JSON_THROW(detail::type_error::create(314, "only objects can be unflattened")); + } + + BasicJsonType result; + + // iterate the JSON object values + for (const auto& element : *value.m_value.object) + { + if (JSON_UNLIKELY(not element.second.is_primitive())) + { + JSON_THROW(detail::type_error::create(315, "values in object must be primitive")); + } + + // assign value to reference pointed to by JSON pointer; Note that if + // the JSON pointer is "" (i.e., points to the whole value), function + // get_and_create returns a reference to result itself. An assignment + // will then create a primitive value. + json_pointer(element.first).get_and_create(result) = element.second; + } + + return result; + } + + friend bool operator==(json_pointer const& lhs, + json_pointer const& rhs) noexcept + { + return (lhs.reference_tokens == rhs.reference_tokens); + } + + friend bool operator!=(json_pointer const& lhs, + json_pointer const& rhs) noexcept + { + return not (lhs == rhs); + } + + /// the reference tokens + std::vector<std::string> reference_tokens; +}; +} // namespace nlohmann + +// #include <nlohmann/adl_serializer.hpp> + + +#include <utility> + +// #include <nlohmann/detail/conversions/from_json.hpp> + +// #include <nlohmann/detail/conversions/to_json.hpp> + + +namespace nlohmann +{ + +template<typename, typename> +struct adl_serializer +{ + /*! + @brief convert a JSON value to any value type + + This function is usually called by the `get()` function of the + @ref basic_json class (either explicit or via conversion operators). + + @param[in] j JSON value to read from + @param[in,out] val value to write to + */ + template<typename BasicJsonType, typename ValueType> + static auto from_json(BasicJsonType&& j, ValueType& val) noexcept( + noexcept(::nlohmann::from_json(std::forward<BasicJsonType>(j), val))) + -> decltype(::nlohmann::from_json(std::forward<BasicJsonType>(j), val), void()) + { + ::nlohmann::from_json(std::forward<BasicJsonType>(j), val); + } + + /*! + @brief convert any value type to a JSON value + + This function is usually called by the constructors of the @ref basic_json + class. + + @param[in,out] j JSON value to write to + @param[in] val value to read from + */ + template <typename BasicJsonType, typename ValueType> + static auto to_json(BasicJsonType& j, ValueType&& val) noexcept( + noexcept(::nlohmann::to_json(j, std::forward<ValueType>(val)))) + -> decltype(::nlohmann::to_json(j, std::forward<ValueType>(val)), void()) + { + ::nlohmann::to_json(j, std::forward<ValueType>(val)); + } +}; + +} // namespace nlohmann + + +/*! +@brief namespace for Niels Lohmann +@see https://github.com/nlohmann +@since version 1.0.0 +*/ +namespace nlohmann +{ + +/*! +@brief a class to store JSON values + +@tparam ObjectType type for JSON objects (`std::map` by default; will be used +in @ref object_t) +@tparam ArrayType type for JSON arrays (`std::vector` by default; will be used +in @ref array_t) +@tparam StringType type for JSON strings and object keys (`std::string` by +default; will be used in @ref string_t) +@tparam BooleanType type for JSON booleans (`bool` by default; will be used +in @ref boolean_t) +@tparam NumberIntegerType type for JSON integer numbers (`int64_t` by +default; will be used in @ref number_integer_t) +@tparam NumberUnsignedType type for JSON unsigned integer numbers (@c +`uint64_t` by default; will be used in @ref number_unsigned_t) +@tparam NumberFloatType type for JSON floating-point numbers (`double` by +default; will be used in @ref number_float_t) +@tparam AllocatorType type of the allocator to use (`std::allocator` by +default) +@tparam JSONSerializer the serializer to resolve internal calls to `to_json()` +and `from_json()` (@ref adl_serializer by default) + +@requirement The class satisfies the following concept requirements: +- Basic + - [DefaultConstructible](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible): + JSON values can be default constructed. The result will be a JSON null + value. + - [MoveConstructible](https://en.cppreference.com/w/cpp/named_req/MoveConstructible): + A JSON value can be constructed from an rvalue argument. + - [CopyConstructible](https://en.cppreference.com/w/cpp/named_req/CopyConstructible): + A JSON value can be copy-constructed from an lvalue expression. + - [MoveAssignable](https://en.cppreference.com/w/cpp/named_req/MoveAssignable): + A JSON value van be assigned from an rvalue argument. + - [CopyAssignable](https://en.cppreference.com/w/cpp/named_req/CopyAssignable): + A JSON value can be copy-assigned from an lvalue expression. + - [Destructible](https://en.cppreference.com/w/cpp/named_req/Destructible): + JSON values can be destructed. +- Layout + - [StandardLayoutType](https://en.cppreference.com/w/cpp/named_req/StandardLayoutType): + JSON values have + [standard layout](https://en.cppreference.com/w/cpp/language/data_members#Standard_layout): + All non-static data members are private and standard layout types, the + class has no virtual functions or (virtual) base classes. +- Library-wide + - [EqualityComparable](https://en.cppreference.com/w/cpp/named_req/EqualityComparable): + JSON values can be compared with `==`, see @ref + operator==(const_reference,const_reference). + - [LessThanComparable](https://en.cppreference.com/w/cpp/named_req/LessThanComparable): + JSON values can be compared with `<`, see @ref + operator<(const_reference,const_reference). + - [Swappable](https://en.cppreference.com/w/cpp/named_req/Swappable): + Any JSON lvalue or rvalue of can be swapped with any lvalue or rvalue of + other compatible types, using unqualified function call @ref swap(). + - [NullablePointer](https://en.cppreference.com/w/cpp/named_req/NullablePointer): + JSON values can be compared against `std::nullptr_t` objects which are used + to model the `null` value. +- Container + - [Container](https://en.cppreference.com/w/cpp/named_req/Container): + JSON values can be used like STL containers and provide iterator access. + - [ReversibleContainer](https://en.cppreference.com/w/cpp/named_req/ReversibleContainer); + JSON values can be used like STL containers and provide reverse iterator + access. + +@invariant The member variables @a m_value and @a m_type have the following +relationship: +- If `m_type == value_t::object`, then `m_value.object != nullptr`. +- If `m_type == value_t::array`, then `m_value.array != nullptr`. +- If `m_type == value_t::string`, then `m_value.string != nullptr`. +The invariants are checked by member function assert_invariant(). + +@internal +@note ObjectType trick from http://stackoverflow.com/a/9860911 +@endinternal + +@see [RFC 7159: The JavaScript Object Notation (JSON) Data Interchange +Format](http://rfc7159.net/rfc7159) + +@since version 1.0.0 + +@nosubgrouping +*/ +NLOHMANN_BASIC_JSON_TPL_DECLARATION +class basic_json +{ + private: + template<detail::value_t> friend struct detail::external_constructor; + friend ::nlohmann::json_pointer<basic_json>; + friend ::nlohmann::detail::parser<basic_json>; + friend ::nlohmann::detail::serializer<basic_json>; + template<typename BasicJsonType> + friend class ::nlohmann::detail::iter_impl; + template<typename BasicJsonType, typename CharType> + friend class ::nlohmann::detail::binary_writer; + template<typename BasicJsonType, typename SAX> + friend class ::nlohmann::detail::binary_reader; + template<typename BasicJsonType> + friend class ::nlohmann::detail::json_sax_dom_parser; + template<typename BasicJsonType> + friend class ::nlohmann::detail::json_sax_dom_callback_parser; + + /// workaround type for MSVC + using basic_json_t = NLOHMANN_BASIC_JSON_TPL; + + // convenience aliases for types residing in namespace detail; + using lexer = ::nlohmann::detail::lexer<basic_json>; + using parser = ::nlohmann::detail::parser<basic_json>; + + using primitive_iterator_t = ::nlohmann::detail::primitive_iterator_t; + template<typename BasicJsonType> + using internal_iterator = ::nlohmann::detail::internal_iterator<BasicJsonType>; + template<typename BasicJsonType> + using iter_impl = ::nlohmann::detail::iter_impl<BasicJsonType>; + template<typename Iterator> + using iteration_proxy = ::nlohmann::detail::iteration_proxy<Iterator>; + template<typename Base> using json_reverse_iterator = ::nlohmann::detail::json_reverse_iterator<Base>; + + template<typename CharType> + using output_adapter_t = ::nlohmann::detail::output_adapter_t<CharType>; + + using binary_reader = ::nlohmann::detail::binary_reader<basic_json>; + template<typename CharType> using binary_writer = ::nlohmann::detail::binary_writer<basic_json, CharType>; + + using serializer = ::nlohmann::detail::serializer<basic_json>; + + public: + using value_t = detail::value_t; + /// JSON Pointer, see @ref nlohmann::json_pointer + using json_pointer = ::nlohmann::json_pointer<basic_json>; + template<typename T, typename SFINAE> + using json_serializer = JSONSerializer<T, SFINAE>; + /// how to treat decoding errors + using error_handler_t = detail::error_handler_t; + /// helper type for initializer lists of basic_json values + using initializer_list_t = std::initializer_list<detail::json_ref<basic_json>>; + + using input_format_t = detail::input_format_t; + /// SAX interface type, see @ref nlohmann::json_sax + using json_sax_t = json_sax<basic_json>; + + //////////////// + // exceptions // + //////////////// + + /// @name exceptions + /// Classes to implement user-defined exceptions. + /// @{ + + /// @copydoc detail::exception + using exception = detail::exception; + /// @copydoc detail::parse_error + using parse_error = detail::parse_error; + /// @copydoc detail::invalid_iterator + using invalid_iterator = detail::invalid_iterator; + /// @copydoc detail::type_error + using type_error = detail::type_error; + /// @copydoc detail::out_of_range + using out_of_range = detail::out_of_range; + /// @copydoc detail::other_error + using other_error = detail::other_error; + + /// @} + + + ///////////////////// + // container types // + ///////////////////// + + /// @name container types + /// The canonic container types to use @ref basic_json like any other STL + /// container. + /// @{ + + /// the type of elements in a basic_json container + using value_type = basic_json; + + /// the type of an element reference + using reference = value_type&; + /// the type of an element const reference + using const_reference = const value_type&; + + /// a type to represent differences between iterators + using difference_type = std::ptrdiff_t; + /// a type to represent container sizes + using size_type = std::size_t; + + /// the allocator type + using allocator_type = AllocatorType<basic_json>; + + /// the type of an element pointer + using pointer = typename std::allocator_traits<allocator_type>::pointer; + /// the type of an element const pointer + using const_pointer = typename std::allocator_traits<allocator_type>::const_pointer; + + /// an iterator for a basic_json container + using iterator = iter_impl<basic_json>; + /// a const iterator for a basic_json container + using const_iterator = iter_impl<const basic_json>; + /// a reverse iterator for a basic_json container + using reverse_iterator = json_reverse_iterator<typename basic_json::iterator>; + /// a const reverse iterator for a basic_json container + using const_reverse_iterator = json_reverse_iterator<typename basic_json::const_iterator>; + + /// @} + + + /*! + @brief returns the allocator associated with the container + */ + static allocator_type get_allocator() + { + return allocator_type(); + } + + /*! + @brief returns version information on the library + + This function returns a JSON object with information about the library, + including the version number and information on the platform and compiler. + + @return JSON object holding version information + key | description + ----------- | --------------- + `compiler` | Information on the used compiler. It is an object with the following keys: `c++` (the used C++ standard), `family` (the compiler family; possible values are `clang`, `icc`, `gcc`, `ilecpp`, `msvc`, `pgcpp`, `sunpro`, and `unknown`), and `version` (the compiler version). + `copyright` | The copyright line for the library as string. + `name` | The name of the library as string. + `platform` | The used platform as string. Possible values are `win32`, `linux`, `apple`, `unix`, and `unknown`. + `url` | The URL of the project as string. + `version` | The version of the library. It is an object with the following keys: `major`, `minor`, and `patch` as defined by [Semantic Versioning](http://semver.org), and `string` (the version string). + + @liveexample{The following code shows an example output of the `meta()` + function.,meta} + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @complexity Constant. + + @since 2.1.0 + */ + static basic_json meta() + { + basic_json result; + + result["copyright"] = "(C) 2013-2017 Niels Lohmann"; + result["name"] = "JSON for Modern C++"; + result["url"] = "https://github.com/nlohmann/json"; + result["version"]["string"] = + std::to_string(NLOHMANN_JSON_VERSION_MAJOR) + "." + + std::to_string(NLOHMANN_JSON_VERSION_MINOR) + "." + + std::to_string(NLOHMANN_JSON_VERSION_PATCH); + result["version"]["major"] = NLOHMANN_JSON_VERSION_MAJOR; + result["version"]["minor"] = NLOHMANN_JSON_VERSION_MINOR; + result["version"]["patch"] = NLOHMANN_JSON_VERSION_PATCH; + +#ifdef _WIN32 + result["platform"] = "win32"; +#elif defined __linux__ + result["platform"] = "linux"; +#elif defined __APPLE__ + result["platform"] = "apple"; +#elif defined __unix__ + result["platform"] = "unix"; +#else + result["platform"] = "unknown"; +#endif + +#if defined(__ICC) || defined(__INTEL_COMPILER) + result["compiler"] = {{"family", "icc"}, {"version", __INTEL_COMPILER}}; +#elif defined(__clang__) + result["compiler"] = {{"family", "clang"}, {"version", __clang_version__}}; +#elif defined(__GNUC__) || defined(__GNUG__) + result["compiler"] = {{"family", "gcc"}, {"version", std::to_string(__GNUC__) + "." + std::to_string(__GNUC_MINOR__) + "." + std::to_string(__GNUC_PATCHLEVEL__)}}; +#elif defined(__HP_cc) || defined(__HP_aCC) + result["compiler"] = "hp" +#elif defined(__IBMCPP__) + result["compiler"] = {{"family", "ilecpp"}, {"version", __IBMCPP__}}; +#elif defined(_MSC_VER) + result["compiler"] = {{"family", "msvc"}, {"version", _MSC_VER}}; +#elif defined(__PGI) + result["compiler"] = {{"family", "pgcpp"}, {"version", __PGI}}; +#elif defined(__SUNPRO_CC) + result["compiler"] = {{"family", "sunpro"}, {"version", __SUNPRO_CC}}; +#else + result["compiler"] = {{"family", "unknown"}, {"version", "unknown"}}; +#endif + +#ifdef __cplusplus + result["compiler"]["c++"] = std::to_string(__cplusplus); +#else + result["compiler"]["c++"] = "unknown"; +#endif + return result; + } + + + /////////////////////////// + // JSON value data types // + /////////////////////////// + + /// @name JSON value data types + /// The data types to store a JSON value. These types are derived from + /// the template arguments passed to class @ref basic_json. + /// @{ + +#if defined(JSON_HAS_CPP_14) + // Use transparent comparator if possible, combined with perfect forwarding + // on find() and count() calls prevents unnecessary string construction. + using object_comparator_t = std::less<>; +#else + using object_comparator_t = std::less<StringType>; +#endif + + /*! + @brief a type for an object + + [RFC 7159](http://rfc7159.net/rfc7159) describes JSON objects as follows: + > An object is an unordered collection of zero or more name/value pairs, + > where a name is a string and a value is a string, number, boolean, null, + > object, or array. + + To store objects in C++, a type is defined by the template parameters + described below. + + @tparam ObjectType the container to store objects (e.g., `std::map` or + `std::unordered_map`) + @tparam StringType the type of the keys or names (e.g., `std::string`). + The comparison function `std::less<StringType>` is used to order elements + inside the container. + @tparam AllocatorType the allocator to use for objects (e.g., + `std::allocator`) + + #### Default type + + With the default values for @a ObjectType (`std::map`), @a StringType + (`std::string`), and @a AllocatorType (`std::allocator`), the default + value for @a object_t is: + + @code {.cpp} + std::map< + std::string, // key_type + basic_json, // value_type + std::less<std::string>, // key_compare + std::allocator<std::pair<const std::string, basic_json>> // allocator_type + > + @endcode + + #### Behavior + + The choice of @a object_t influences the behavior of the JSON class. With + the default type, objects have the following behavior: + + - When all names are unique, objects will be interoperable in the sense + that all software implementations receiving that object will agree on + the name-value mappings. + - When the names within an object are not unique, it is unspecified which + one of the values for a given key will be chosen. For instance, + `{"key": 2, "key": 1}` could be equal to either `{"key": 1}` or + `{"key": 2}`. + - Internally, name/value pairs are stored in lexicographical order of the + names. Objects will also be serialized (see @ref dump) in this order. + For instance, `{"b": 1, "a": 2}` and `{"a": 2, "b": 1}` will be stored + and serialized as `{"a": 2, "b": 1}`. + - When comparing objects, the order of the name/value pairs is irrelevant. + This makes objects interoperable in the sense that they will not be + affected by these differences. For instance, `{"b": 1, "a": 2}` and + `{"a": 2, "b": 1}` will be treated as equal. + + #### Limits + + [RFC 7159](http://rfc7159.net/rfc7159) specifies: + > An implementation may set limits on the maximum depth of nesting. + + In this class, the object's limit of nesting is not explicitly constrained. + However, a maximum depth of nesting may be introduced by the compiler or + runtime environment. A theoretical limit can be queried by calling the + @ref max_size function of a JSON object. + + #### Storage + + Objects are stored as pointers in a @ref basic_json type. That is, for any + access to object values, a pointer of type `object_t*` must be + dereferenced. + + @sa @ref array_t -- type for an array value + + @since version 1.0.0 + + @note The order name/value pairs are added to the object is *not* + preserved by the library. Therefore, iterating an object may return + name/value pairs in a different order than they were originally stored. In + fact, keys will be traversed in alphabetical order as `std::map` with + `std::less` is used by default. Please note this behavior conforms to [RFC + 7159](http://rfc7159.net/rfc7159), because any order implements the + specified "unordered" nature of JSON objects. + */ + using object_t = ObjectType<StringType, + basic_json, + object_comparator_t, + AllocatorType<std::pair<const StringType, + basic_json>>>; + + /*! + @brief a type for an array + + [RFC 7159](http://rfc7159.net/rfc7159) describes JSON arrays as follows: + > An array is an ordered sequence of zero or more values. + + To store objects in C++, a type is defined by the template parameters + explained below. + + @tparam ArrayType container type to store arrays (e.g., `std::vector` or + `std::list`) + @tparam AllocatorType allocator to use for arrays (e.g., `std::allocator`) + + #### Default type + + With the default values for @a ArrayType (`std::vector`) and @a + AllocatorType (`std::allocator`), the default value for @a array_t is: + + @code {.cpp} + std::vector< + basic_json, // value_type + std::allocator<basic_json> // allocator_type + > + @endcode + + #### Limits + + [RFC 7159](http://rfc7159.net/rfc7159) specifies: + > An implementation may set limits on the maximum depth of nesting. + + In this class, the array's limit of nesting is not explicitly constrained. + However, a maximum depth of nesting may be introduced by the compiler or + runtime environment. A theoretical limit can be queried by calling the + @ref max_size function of a JSON array. + + #### Storage + + Arrays are stored as pointers in a @ref basic_json type. That is, for any + access to array values, a pointer of type `array_t*` must be dereferenced. + + @sa @ref object_t -- type for an object value + + @since version 1.0.0 + */ + using array_t = ArrayType<basic_json, AllocatorType<basic_json>>; + + /*! + @brief a type for a string + + [RFC 7159](http://rfc7159.net/rfc7159) describes JSON strings as follows: + > A string is a sequence of zero or more Unicode characters. + + To store objects in C++, a type is defined by the template parameter + described below. Unicode values are split by the JSON class into + byte-sized characters during deserialization. + + @tparam StringType the container to store strings (e.g., `std::string`). + Note this container is used for keys/names in objects, see @ref object_t. + + #### Default type + + With the default values for @a StringType (`std::string`), the default + value for @a string_t is: + + @code {.cpp} + std::string + @endcode + + #### Encoding + + Strings are stored in UTF-8 encoding. Therefore, functions like + `std::string::size()` or `std::string::length()` return the number of + bytes in the string rather than the number of characters or glyphs. + + #### String comparison + + [RFC 7159](http://rfc7159.net/rfc7159) states: + > Software implementations are typically required to test names of object + > members for equality. Implementations that transform the textual + > representation into sequences of Unicode code units and then perform the + > comparison numerically, code unit by code unit, are interoperable in the + > sense that implementations will agree in all cases on equality or + > inequality of two strings. For example, implementations that compare + > strings with escaped characters unconverted may incorrectly find that + > `"a\\b"` and `"a\u005Cb"` are not equal. + + This implementation is interoperable as it does compare strings code unit + by code unit. + + #### Storage + + String values are stored as pointers in a @ref basic_json type. That is, + for any access to string values, a pointer of type `string_t*` must be + dereferenced. + + @since version 1.0.0 + */ + using string_t = StringType; + + /*! + @brief a type for a boolean + + [RFC 7159](http://rfc7159.net/rfc7159) implicitly describes a boolean as a + type which differentiates the two literals `true` and `false`. + + To store objects in C++, a type is defined by the template parameter @a + BooleanType which chooses the type to use. + + #### Default type + + With the default values for @a BooleanType (`bool`), the default value for + @a boolean_t is: + + @code {.cpp} + bool + @endcode + + #### Storage + + Boolean values are stored directly inside a @ref basic_json type. + + @since version 1.0.0 + */ + using boolean_t = BooleanType; + + /*! + @brief a type for a number (integer) + + [RFC 7159](http://rfc7159.net/rfc7159) describes numbers as follows: + > The representation of numbers is similar to that used in most + > programming languages. A number is represented in base 10 using decimal + > digits. It contains an integer component that may be prefixed with an + > optional minus sign, which may be followed by a fraction part and/or an + > exponent part. Leading zeros are not allowed. (...) Numeric values that + > cannot be represented in the grammar below (such as Infinity and NaN) + > are not permitted. + + This description includes both integer and floating-point numbers. + However, C++ allows more precise storage if it is known whether the number + is a signed integer, an unsigned integer or a floating-point number. + Therefore, three different types, @ref number_integer_t, @ref + number_unsigned_t and @ref number_float_t are used. + + To store integer numbers in C++, a type is defined by the template + parameter @a NumberIntegerType which chooses the type to use. + + #### Default type + + With the default values for @a NumberIntegerType (`int64_t`), the default + value for @a number_integer_t is: + + @code {.cpp} + int64_t + @endcode + + #### Default behavior + + - The restrictions about leading zeros is not enforced in C++. Instead, + leading zeros in integer literals lead to an interpretation as octal + number. Internally, the value will be stored as decimal number. For + instance, the C++ integer literal `010` will be serialized to `8`. + During deserialization, leading zeros yield an error. + - Not-a-number (NaN) values will be serialized to `null`. + + #### Limits + + [RFC 7159](http://rfc7159.net/rfc7159) specifies: + > An implementation may set limits on the range and precision of numbers. + + When the default type is used, the maximal integer number that can be + stored is `9223372036854775807` (INT64_MAX) and the minimal integer number + that can be stored is `-9223372036854775808` (INT64_MIN). Integer numbers + that are out of range will yield over/underflow when used in a + constructor. During deserialization, too large or small integer numbers + will be automatically be stored as @ref number_unsigned_t or @ref + number_float_t. + + [RFC 7159](http://rfc7159.net/rfc7159) further states: + > Note that when such software is used, numbers that are integers and are + > in the range \f$[-2^{53}+1, 2^{53}-1]\f$ are interoperable in the sense + > that implementations will agree exactly on their numeric values. + + As this range is a subrange of the exactly supported range [INT64_MIN, + INT64_MAX], this class's integer type is interoperable. + + #### Storage + + Integer number values are stored directly inside a @ref basic_json type. + + @sa @ref number_float_t -- type for number values (floating-point) + + @sa @ref number_unsigned_t -- type for number values (unsigned integer) + + @since version 1.0.0 + */ + using number_integer_t = NumberIntegerType; + + /*! + @brief a type for a number (unsigned) + + [RFC 7159](http://rfc7159.net/rfc7159) describes numbers as follows: + > The representation of numbers is similar to that used in most + > programming languages. A number is represented in base 10 using decimal + > digits. It contains an integer component that may be prefixed with an + > optional minus sign, which may be followed by a fraction part and/or an + > exponent part. Leading zeros are not allowed. (...) Numeric values that + > cannot be represented in the grammar below (such as Infinity and NaN) + > are not permitted. + + This description includes both integer and floating-point numbers. + However, C++ allows more precise storage if it is known whether the number + is a signed integer, an unsigned integer or a floating-point number. + Therefore, three different types, @ref number_integer_t, @ref + number_unsigned_t and @ref number_float_t are used. + + To store unsigned integer numbers in C++, a type is defined by the + template parameter @a NumberUnsignedType which chooses the type to use. + + #### Default type + + With the default values for @a NumberUnsignedType (`uint64_t`), the + default value for @a number_unsigned_t is: + + @code {.cpp} + uint64_t + @endcode + + #### Default behavior + + - The restrictions about leading zeros is not enforced in C++. Instead, + leading zeros in integer literals lead to an interpretation as octal + number. Internally, the value will be stored as decimal number. For + instance, the C++ integer literal `010` will be serialized to `8`. + During deserialization, leading zeros yield an error. + - Not-a-number (NaN) values will be serialized to `null`. + + #### Limits + + [RFC 7159](http://rfc7159.net/rfc7159) specifies: + > An implementation may set limits on the range and precision of numbers. + + When the default type is used, the maximal integer number that can be + stored is `18446744073709551615` (UINT64_MAX) and the minimal integer + number that can be stored is `0`. Integer numbers that are out of range + will yield over/underflow when used in a constructor. During + deserialization, too large or small integer numbers will be automatically + be stored as @ref number_integer_t or @ref number_float_t. + + [RFC 7159](http://rfc7159.net/rfc7159) further states: + > Note that when such software is used, numbers that are integers and are + > in the range \f$[-2^{53}+1, 2^{53}-1]\f$ are interoperable in the sense + > that implementations will agree exactly on their numeric values. + + As this range is a subrange (when considered in conjunction with the + number_integer_t type) of the exactly supported range [0, UINT64_MAX], + this class's integer type is interoperable. + + #### Storage + + Integer number values are stored directly inside a @ref basic_json type. + + @sa @ref number_float_t -- type for number values (floating-point) + @sa @ref number_integer_t -- type for number values (integer) + + @since version 2.0.0 + */ + using number_unsigned_t = NumberUnsignedType; + + /*! + @brief a type for a number (floating-point) + + [RFC 7159](http://rfc7159.net/rfc7159) describes numbers as follows: + > The representation of numbers is similar to that used in most + > programming languages. A number is represented in base 10 using decimal + > digits. It contains an integer component that may be prefixed with an + > optional minus sign, which may be followed by a fraction part and/or an + > exponent part. Leading zeros are not allowed. (...) Numeric values that + > cannot be represented in the grammar below (such as Infinity and NaN) + > are not permitted. + + This description includes both integer and floating-point numbers. + However, C++ allows more precise storage if it is known whether the number + is a signed integer, an unsigned integer or a floating-point number. + Therefore, three different types, @ref number_integer_t, @ref + number_unsigned_t and @ref number_float_t are used. + + To store floating-point numbers in C++, a type is defined by the template + parameter @a NumberFloatType which chooses the type to use. + + #### Default type + + With the default values for @a NumberFloatType (`double`), the default + value for @a number_float_t is: + + @code {.cpp} + double + @endcode + + #### Default behavior + + - The restrictions about leading zeros is not enforced in C++. Instead, + leading zeros in floating-point literals will be ignored. Internally, + the value will be stored as decimal number. For instance, the C++ + floating-point literal `01.2` will be serialized to `1.2`. During + deserialization, leading zeros yield an error. + - Not-a-number (NaN) values will be serialized to `null`. + + #### Limits + + [RFC 7159](http://rfc7159.net/rfc7159) states: + > This specification allows implementations to set limits on the range and + > precision of numbers accepted. Since software that implements IEEE + > 754-2008 binary64 (double precision) numbers is generally available and + > widely used, good interoperability can be achieved by implementations + > that expect no more precision or range than these provide, in the sense + > that implementations will approximate JSON numbers within the expected + > precision. + + This implementation does exactly follow this approach, as it uses double + precision floating-point numbers. Note values smaller than + `-1.79769313486232e+308` and values greater than `1.79769313486232e+308` + will be stored as NaN internally and be serialized to `null`. + + #### Storage + + Floating-point number values are stored directly inside a @ref basic_json + type. + + @sa @ref number_integer_t -- type for number values (integer) + + @sa @ref number_unsigned_t -- type for number values (unsigned integer) + + @since version 1.0.0 + */ + using number_float_t = NumberFloatType; + + /// @} + + private: + + /// helper for exception-safe object creation + template<typename T, typename... Args> + static T* create(Args&& ... args) + { + AllocatorType<T> alloc; + using AllocatorTraits = std::allocator_traits<AllocatorType<T>>; + + auto deleter = [&](T * object) + { + AllocatorTraits::deallocate(alloc, object, 1); + }; + std::unique_ptr<T, decltype(deleter)> object(AllocatorTraits::allocate(alloc, 1), deleter); + AllocatorTraits::construct(alloc, object.get(), std::forward<Args>(args)...); + assert(object != nullptr); + return object.release(); + } + + //////////////////////// + // JSON value storage // + //////////////////////// + + /*! + @brief a JSON value + + The actual storage for a JSON value of the @ref basic_json class. This + union combines the different storage types for the JSON value types + defined in @ref value_t. + + JSON type | value_t type | used type + --------- | --------------- | ------------------------ + object | object | pointer to @ref object_t + array | array | pointer to @ref array_t + string | string | pointer to @ref string_t + boolean | boolean | @ref boolean_t + number | number_integer | @ref number_integer_t + number | number_unsigned | @ref number_unsigned_t + number | number_float | @ref number_float_t + null | null | *no value is stored* + + @note Variable-length types (objects, arrays, and strings) are stored as + pointers. The size of the union should not exceed 64 bits if the default + value types are used. + + @since version 1.0.0 + */ + union json_value + { + /// object (stored with pointer to save storage) + object_t* object; + /// array (stored with pointer to save storage) + array_t* array; + /// string (stored with pointer to save storage) + string_t* string; + /// boolean + boolean_t boolean; + /// number (integer) + number_integer_t number_integer; + /// number (unsigned integer) + number_unsigned_t number_unsigned; + /// number (floating-point) + number_float_t number_float; + + /// default constructor (for null values) + json_value() = default; + /// constructor for booleans + json_value(boolean_t v) noexcept : boolean(v) {} + /// constructor for numbers (integer) + json_value(number_integer_t v) noexcept : number_integer(v) {} + /// constructor for numbers (unsigned) + json_value(number_unsigned_t v) noexcept : number_unsigned(v) {} + /// constructor for numbers (floating-point) + json_value(number_float_t v) noexcept : number_float(v) {} + /// constructor for empty values of a given type + json_value(value_t t) + { + switch (t) + { + case value_t::object: + { + object = create<object_t>(); + break; + } + + case value_t::array: + { + array = create<array_t>(); + break; + } + + case value_t::string: + { + string = create<string_t>(""); + break; + } + + case value_t::boolean: + { + boolean = boolean_t(false); + break; + } + + case value_t::number_integer: + { + number_integer = number_integer_t(0); + break; + } + + case value_t::number_unsigned: + { + number_unsigned = number_unsigned_t(0); + break; + } + + case value_t::number_float: + { + number_float = number_float_t(0.0); + break; + } + + case value_t::null: + { + object = nullptr; // silence warning, see #821 + break; + } + + default: + { + object = nullptr; // silence warning, see #821 + if (JSON_UNLIKELY(t == value_t::null)) + { + JSON_THROW(other_error::create(500, "961c151d2e87f2686a955a9be24d316f1362bf21 3.4.0")); // LCOV_EXCL_LINE + } + break; + } + } + } + + /// constructor for strings + json_value(const string_t& value) + { + string = create<string_t>(value); + } + + /// constructor for rvalue strings + json_value(string_t&& value) + { + string = create<string_t>(std::move(value)); + } + + /// constructor for objects + json_value(const object_t& value) + { + object = create<object_t>(value); + } + + /// constructor for rvalue objects + json_value(object_t&& value) + { + object = create<object_t>(std::move(value)); + } + + /// constructor for arrays + json_value(const array_t& value) + { + array = create<array_t>(value); + } + + /// constructor for rvalue arrays + json_value(array_t&& value) + { + array = create<array_t>(std::move(value)); + } + + void destroy(value_t t) noexcept + { + switch (t) + { + case value_t::object: + { + AllocatorType<object_t> alloc; + std::allocator_traits<decltype(alloc)>::destroy(alloc, object); + std::allocator_traits<decltype(alloc)>::deallocate(alloc, object, 1); + break; + } + + case value_t::array: + { + AllocatorType<array_t> alloc; + std::allocator_traits<decltype(alloc)>::destroy(alloc, array); + std::allocator_traits<decltype(alloc)>::deallocate(alloc, array, 1); + break; + } + + case value_t::string: + { + AllocatorType<string_t> alloc; + std::allocator_traits<decltype(alloc)>::destroy(alloc, string); + std::allocator_traits<decltype(alloc)>::deallocate(alloc, string, 1); + break; + } + + default: + { + break; + } + } + } + }; + + /*! + @brief checks the class invariants + + This function asserts the class invariants. It needs to be called at the + end of every constructor to make sure that created objects respect the + invariant. Furthermore, it has to be called each time the type of a JSON + value is changed, because the invariant expresses a relationship between + @a m_type and @a m_value. + */ + void assert_invariant() const noexcept + { + assert(m_type != value_t::object or m_value.object != nullptr); + assert(m_type != value_t::array or m_value.array != nullptr); + assert(m_type != value_t::string or m_value.string != nullptr); + } + + public: + ////////////////////////// + // JSON parser callback // + ////////////////////////// + + /*! + @brief parser event types + + The parser callback distinguishes the following events: + - `object_start`: the parser read `{` and started to process a JSON object + - `key`: the parser read a key of a value in an object + - `object_end`: the parser read `}` and finished processing a JSON object + - `array_start`: the parser read `[` and started to process a JSON array + - `array_end`: the parser read `]` and finished processing a JSON array + - `value`: the parser finished reading a JSON value + + @image html callback_events.png "Example when certain parse events are triggered" + + @sa @ref parser_callback_t for more information and examples + */ + using parse_event_t = typename parser::parse_event_t; + + /*! + @brief per-element parser callback type + + With a parser callback function, the result of parsing a JSON text can be + influenced. When passed to @ref parse, it is called on certain events + (passed as @ref parse_event_t via parameter @a event) with a set recursion + depth @a depth and context JSON value @a parsed. The return value of the + callback function is a boolean indicating whether the element that emitted + the callback shall be kept or not. + + We distinguish six scenarios (determined by the event type) in which the + callback function can be called. The following table describes the values + of the parameters @a depth, @a event, and @a parsed. + + parameter @a event | description | parameter @a depth | parameter @a parsed + ------------------ | ----------- | ------------------ | ------------------- + parse_event_t::object_start | the parser read `{` and started to process a JSON object | depth of the parent of the JSON object | a JSON value with type discarded + parse_event_t::key | the parser read a key of a value in an object | depth of the currently parsed JSON object | a JSON string containing the key + parse_event_t::object_end | the parser read `}` and finished processing a JSON object | depth of the parent of the JSON object | the parsed JSON object + parse_event_t::array_start | the parser read `[` and started to process a JSON array | depth of the parent of the JSON array | a JSON value with type discarded + parse_event_t::array_end | the parser read `]` and finished processing a JSON array | depth of the parent of the JSON array | the parsed JSON array + parse_event_t::value | the parser finished reading a JSON value | depth of the value | the parsed JSON value + + @image html callback_events.png "Example when certain parse events are triggered" + + Discarding a value (i.e., returning `false`) has different effects + depending on the context in which function was called: + + - Discarded values in structured types are skipped. That is, the parser + will behave as if the discarded value was never read. + - In case a value outside a structured type is skipped, it is replaced + with `null`. This case happens if the top-level element is skipped. + + @param[in] depth the depth of the recursion during parsing + + @param[in] event an event of type parse_event_t indicating the context in + the callback function has been called + + @param[in,out] parsed the current intermediate parse result; note that + writing to this value has no effect for parse_event_t::key events + + @return Whether the JSON value which called the function during parsing + should be kept (`true`) or not (`false`). In the latter case, it is either + skipped completely or replaced by an empty discarded object. + + @sa @ref parse for examples + + @since version 1.0.0 + */ + using parser_callback_t = typename parser::parser_callback_t; + + ////////////////// + // constructors // + ////////////////// + + /// @name constructors and destructors + /// Constructors of class @ref basic_json, copy/move constructor, copy + /// assignment, static functions creating objects, and the destructor. + /// @{ + + /*! + @brief create an empty value with a given type + + Create an empty JSON value with a given type. The value will be default + initialized with an empty value which depends on the type: + + Value type | initial value + ----------- | ------------- + null | `null` + boolean | `false` + string | `""` + number | `0` + object | `{}` + array | `[]` + + @param[in] v the type of the value to create + + @complexity Constant. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @liveexample{The following code shows the constructor for different @ref + value_t values,basic_json__value_t} + + @sa @ref clear() -- restores the postcondition of this constructor + + @since version 1.0.0 + */ + basic_json(const value_t v) + : m_type(v), m_value(v) + { + assert_invariant(); + } + + /*! + @brief create a null object + + Create a `null` JSON value. It either takes a null pointer as parameter + (explicitly creating `null`) or no parameter (implicitly creating `null`). + The passed null pointer itself is not read -- it is only used to choose + the right constructor. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this constructor never throws + exceptions. + + @liveexample{The following code shows the constructor with and without a + null pointer parameter.,basic_json__nullptr_t} + + @since version 1.0.0 + */ + basic_json(std::nullptr_t = nullptr) noexcept + : basic_json(value_t::null) + { + assert_invariant(); + } + + /*! + @brief create a JSON value + + This is a "catch all" constructor for all compatible JSON types; that is, + types for which a `to_json()` method exists. The constructor forwards the + parameter @a val to that method (to `json_serializer<U>::to_json` method + with `U = uncvref_t<CompatibleType>`, to be exact). + + Template type @a CompatibleType includes, but is not limited to, the + following types: + - **arrays**: @ref array_t and all kinds of compatible containers such as + `std::vector`, `std::deque`, `std::list`, `std::forward_list`, + `std::array`, `std::valarray`, `std::set`, `std::unordered_set`, + `std::multiset`, and `std::unordered_multiset` with a `value_type` from + which a @ref basic_json value can be constructed. + - **objects**: @ref object_t and all kinds of compatible associative + containers such as `std::map`, `std::unordered_map`, `std::multimap`, + and `std::unordered_multimap` with a `key_type` compatible to + @ref string_t and a `value_type` from which a @ref basic_json value can + be constructed. + - **strings**: @ref string_t, string literals, and all compatible string + containers can be used. + - **numbers**: @ref number_integer_t, @ref number_unsigned_t, + @ref number_float_t, and all convertible number types such as `int`, + `size_t`, `int64_t`, `float` or `double` can be used. + - **boolean**: @ref boolean_t / `bool` can be used. + + See the examples below. + + @tparam CompatibleType a type such that: + - @a CompatibleType is not derived from `std::istream`, + - @a CompatibleType is not @ref basic_json (to avoid hijacking copy/move + constructors), + - @a CompatibleType is not a different @ref basic_json type (i.e. with different template arguments) + - @a CompatibleType is not a @ref basic_json nested type (e.g., + @ref json_pointer, @ref iterator, etc ...) + - @ref @ref json_serializer<U> has a + `to_json(basic_json_t&, CompatibleType&&)` method + + @tparam U = `uncvref_t<CompatibleType>` + + @param[in] val the value to be forwarded to the respective constructor + + @complexity Usually linear in the size of the passed @a val, also + depending on the implementation of the called `to_json()` + method. + + @exceptionsafety Depends on the called constructor. For types directly + supported by the library (i.e., all types for which no `to_json()` function + was provided), strong guarantee holds: if an exception is thrown, there are + no changes to any JSON value. + + @liveexample{The following code shows the constructor with several + compatible types.,basic_json__CompatibleType} + + @since version 2.1.0 + */ + template <typename CompatibleType, + typename U = detail::uncvref_t<CompatibleType>, + detail::enable_if_t< + not detail::is_basic_json<U>::value and detail::is_compatible_type<basic_json_t, U>::value, int> = 0> + basic_json(CompatibleType && val) noexcept(noexcept( + JSONSerializer<U>::to_json(std::declval<basic_json_t&>(), + std::forward<CompatibleType>(val)))) + { + JSONSerializer<U>::to_json(*this, std::forward<CompatibleType>(val)); + assert_invariant(); + } + + /*! + @brief create a JSON value from an existing one + + This is a constructor for existing @ref basic_json types. + It does not hijack copy/move constructors, since the parameter has different + template arguments than the current ones. + + The constructor tries to convert the internal @ref m_value of the parameter. + + @tparam BasicJsonType a type such that: + - @a BasicJsonType is a @ref basic_json type. + - @a BasicJsonType has different template arguments than @ref basic_json_t. + + @param[in] val the @ref basic_json value to be converted. + + @complexity Usually linear in the size of the passed @a val, also + depending on the implementation of the called `to_json()` + method. + + @exceptionsafety Depends on the called constructor. For types directly + supported by the library (i.e., all types for which no `to_json()` function + was provided), strong guarantee holds: if an exception is thrown, there are + no changes to any JSON value. + + @since version 3.2.0 + */ + template <typename BasicJsonType, + detail::enable_if_t< + detail::is_basic_json<BasicJsonType>::value and not std::is_same<basic_json, BasicJsonType>::value, int> = 0> + basic_json(const BasicJsonType& val) + { + using other_boolean_t = typename BasicJsonType::boolean_t; + using other_number_float_t = typename BasicJsonType::number_float_t; + using other_number_integer_t = typename BasicJsonType::number_integer_t; + using other_number_unsigned_t = typename BasicJsonType::number_unsigned_t; + using other_string_t = typename BasicJsonType::string_t; + using other_object_t = typename BasicJsonType::object_t; + using other_array_t = typename BasicJsonType::array_t; + + switch (val.type()) + { + case value_t::boolean: + JSONSerializer<other_boolean_t>::to_json(*this, val.template get<other_boolean_t>()); + break; + case value_t::number_float: + JSONSerializer<other_number_float_t>::to_json(*this, val.template get<other_number_float_t>()); + break; + case value_t::number_integer: + JSONSerializer<other_number_integer_t>::to_json(*this, val.template get<other_number_integer_t>()); + break; + case value_t::number_unsigned: + JSONSerializer<other_number_unsigned_t>::to_json(*this, val.template get<other_number_unsigned_t>()); + break; + case value_t::string: + JSONSerializer<other_string_t>::to_json(*this, val.template get_ref<const other_string_t&>()); + break; + case value_t::object: + JSONSerializer<other_object_t>::to_json(*this, val.template get_ref<const other_object_t&>()); + break; + case value_t::array: + JSONSerializer<other_array_t>::to_json(*this, val.template get_ref<const other_array_t&>()); + break; + case value_t::null: + *this = nullptr; + break; + case value_t::discarded: + m_type = value_t::discarded; + break; + } + assert_invariant(); + } + + /*! + @brief create a container (array or object) from an initializer list + + Creates a JSON value of type array or object from the passed initializer + list @a init. In case @a type_deduction is `true` (default), the type of + the JSON value to be created is deducted from the initializer list @a init + according to the following rules: + + 1. If the list is empty, an empty JSON object value `{}` is created. + 2. If the list consists of pairs whose first element is a string, a JSON + object value is created where the first elements of the pairs are + treated as keys and the second elements are as values. + 3. In all other cases, an array is created. + + The rules aim to create the best fit between a C++ initializer list and + JSON values. The rationale is as follows: + + 1. The empty initializer list is written as `{}` which is exactly an empty + JSON object. + 2. C++ has no way of describing mapped types other than to list a list of + pairs. As JSON requires that keys must be of type string, rule 2 is the + weakest constraint one can pose on initializer lists to interpret them + as an object. + 3. In all other cases, the initializer list could not be interpreted as + JSON object type, so interpreting it as JSON array type is safe. + + With the rules described above, the following JSON values cannot be + expressed by an initializer list: + + - the empty array (`[]`): use @ref array(initializer_list_t) + with an empty initializer list in this case + - arrays whose elements satisfy rule 2: use @ref + array(initializer_list_t) with the same initializer list + in this case + + @note When used without parentheses around an empty initializer list, @ref + basic_json() is called instead of this function, yielding the JSON null + value. + + @param[in] init initializer list with JSON values + + @param[in] type_deduction internal parameter; when set to `true`, the type + of the JSON value is deducted from the initializer list @a init; when set + to `false`, the type provided via @a manual_type is forced. This mode is + used by the functions @ref array(initializer_list_t) and + @ref object(initializer_list_t). + + @param[in] manual_type internal parameter; when @a type_deduction is set + to `false`, the created JSON value will use the provided type (only @ref + value_t::array and @ref value_t::object are valid); when @a type_deduction + is set to `true`, this parameter has no effect + + @throw type_error.301 if @a type_deduction is `false`, @a manual_type is + `value_t::object`, but @a init contains an element which is not a pair + whose first element is a string. In this case, the constructor could not + create an object. If @a type_deduction would have be `true`, an array + would have been created. See @ref object(initializer_list_t) + for an example. + + @complexity Linear in the size of the initializer list @a init. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @liveexample{The example below shows how JSON values are created from + initializer lists.,basic_json__list_init_t} + + @sa @ref array(initializer_list_t) -- create a JSON array + value from an initializer list + @sa @ref object(initializer_list_t) -- create a JSON object + value from an initializer list + + @since version 1.0.0 + */ + basic_json(initializer_list_t init, + bool type_deduction = true, + value_t manual_type = value_t::array) + { + // check if each element is an array with two elements whose first + // element is a string + bool is_an_object = std::all_of(init.begin(), init.end(), + [](const detail::json_ref<basic_json>& element_ref) + { + return (element_ref->is_array() and element_ref->size() == 2 and (*element_ref)[0].is_string()); + }); + + // adjust type if type deduction is not wanted + if (not type_deduction) + { + // if array is wanted, do not create an object though possible + if (manual_type == value_t::array) + { + is_an_object = false; + } + + // if object is wanted but impossible, throw an exception + if (JSON_UNLIKELY(manual_type == value_t::object and not is_an_object)) + { + JSON_THROW(type_error::create(301, "cannot create object from initializer list")); + } + } + + if (is_an_object) + { + // the initializer list is a list of pairs -> create object + m_type = value_t::object; + m_value = value_t::object; + + std::for_each(init.begin(), init.end(), [this](const detail::json_ref<basic_json>& element_ref) + { + auto element = element_ref.moved_or_copied(); + m_value.object->emplace( + std::move(*((*element.m_value.array)[0].m_value.string)), + std::move((*element.m_value.array)[1])); + }); + } + else + { + // the initializer list describes an array -> create array + m_type = value_t::array; + m_value.array = create<array_t>(init.begin(), init.end()); + } + + assert_invariant(); + } + + /*! + @brief explicitly create an array from an initializer list + + Creates a JSON array value from a given initializer list. That is, given a + list of values `a, b, c`, creates the JSON value `[a, b, c]`. If the + initializer list is empty, the empty array `[]` is created. + + @note This function is only needed to express two edge cases that cannot + be realized with the initializer list constructor (@ref + basic_json(initializer_list_t, bool, value_t)). These cases + are: + 1. creating an array whose elements are all pairs whose first element is a + string -- in this case, the initializer list constructor would create an + object, taking the first elements as keys + 2. creating an empty array -- passing the empty initializer list to the + initializer list constructor yields an empty object + + @param[in] init initializer list with JSON values to create an array from + (optional) + + @return JSON array value + + @complexity Linear in the size of @a init. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @liveexample{The following code shows an example for the `array` + function.,array} + + @sa @ref basic_json(initializer_list_t, bool, value_t) -- + create a JSON value from an initializer list + @sa @ref object(initializer_list_t) -- create a JSON object + value from an initializer list + + @since version 1.0.0 + */ + static basic_json array(initializer_list_t init = {}) + { + return basic_json(init, false, value_t::array); + } + + /*! + @brief explicitly create an object from an initializer list + + Creates a JSON object value from a given initializer list. The initializer + lists elements must be pairs, and their first elements must be strings. If + the initializer list is empty, the empty object `{}` is created. + + @note This function is only added for symmetry reasons. In contrast to the + related function @ref array(initializer_list_t), there are + no cases which can only be expressed by this function. That is, any + initializer list @a init can also be passed to the initializer list + constructor @ref basic_json(initializer_list_t, bool, value_t). + + @param[in] init initializer list to create an object from (optional) + + @return JSON object value + + @throw type_error.301 if @a init is not a list of pairs whose first + elements are strings. In this case, no object can be created. When such a + value is passed to @ref basic_json(initializer_list_t, bool, value_t), + an array would have been created from the passed initializer list @a init. + See example below. + + @complexity Linear in the size of @a init. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @liveexample{The following code shows an example for the `object` + function.,object} + + @sa @ref basic_json(initializer_list_t, bool, value_t) -- + create a JSON value from an initializer list + @sa @ref array(initializer_list_t) -- create a JSON array + value from an initializer list + + @since version 1.0.0 + */ + static basic_json object(initializer_list_t init = {}) + { + return basic_json(init, false, value_t::object); + } + + /*! + @brief construct an array with count copies of given value + + Constructs a JSON array value by creating @a cnt copies of a passed value. + In case @a cnt is `0`, an empty array is created. + + @param[in] cnt the number of JSON copies of @a val to create + @param[in] val the JSON value to copy + + @post `std::distance(begin(),end()) == cnt` holds. + + @complexity Linear in @a cnt. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @liveexample{The following code shows examples for the @ref + basic_json(size_type\, const basic_json&) + constructor.,basic_json__size_type_basic_json} + + @since version 1.0.0 + */ + basic_json(size_type cnt, const basic_json& val) + : m_type(value_t::array) + { + m_value.array = create<array_t>(cnt, val); + assert_invariant(); + } + + /*! + @brief construct a JSON container given an iterator range + + Constructs the JSON value with the contents of the range `[first, last)`. + The semantics depends on the different types a JSON value can have: + - In case of a null type, invalid_iterator.206 is thrown. + - In case of other primitive types (number, boolean, or string), @a first + must be `begin()` and @a last must be `end()`. In this case, the value is + copied. Otherwise, invalid_iterator.204 is thrown. + - In case of structured types (array, object), the constructor behaves as + similar versions for `std::vector` or `std::map`; that is, a JSON array + or object is constructed from the values in the range. + + @tparam InputIT an input iterator type (@ref iterator or @ref + const_iterator) + + @param[in] first begin of the range to copy from (included) + @param[in] last end of the range to copy from (excluded) + + @pre Iterators @a first and @a last must be initialized. **This + precondition is enforced with an assertion (see warning).** If + assertions are switched off, a violation of this precondition yields + undefined behavior. + + @pre Range `[first, last)` is valid. Usually, this precondition cannot be + checked efficiently. Only certain edge cases are detected; see the + description of the exceptions below. A violation of this precondition + yields undefined behavior. + + @warning A precondition is enforced with a runtime assertion that will + result in calling `std::abort` if this precondition is not met. + Assertions can be disabled by defining `NDEBUG` at compile time. + See https://en.cppreference.com/w/cpp/error/assert for more + information. + + @throw invalid_iterator.201 if iterators @a first and @a last are not + compatible (i.e., do not belong to the same JSON value). In this case, + the range `[first, last)` is undefined. + @throw invalid_iterator.204 if iterators @a first and @a last belong to a + primitive type (number, boolean, or string), but @a first does not point + to the first element any more. In this case, the range `[first, last)` is + undefined. See example code below. + @throw invalid_iterator.206 if iterators @a first and @a last belong to a + null value. In this case, the range `[first, last)` is undefined. + + @complexity Linear in distance between @a first and @a last. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @liveexample{The example below shows several ways to create JSON values by + specifying a subrange with iterators.,basic_json__InputIt_InputIt} + + @since version 1.0.0 + */ + template<class InputIT, typename std::enable_if< + std::is_same<InputIT, typename basic_json_t::iterator>::value or + std::is_same<InputIT, typename basic_json_t::const_iterator>::value, int>::type = 0> + basic_json(InputIT first, InputIT last) + { + assert(first.m_object != nullptr); + assert(last.m_object != nullptr); + + // make sure iterator fits the current value + if (JSON_UNLIKELY(first.m_object != last.m_object)) + { + JSON_THROW(invalid_iterator::create(201, "iterators are not compatible")); + } + + // copy type from first iterator + m_type = first.m_object->m_type; + + // check if iterator range is complete for primitive values + switch (m_type) + { + case value_t::boolean: + case value_t::number_float: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::string: + { + if (JSON_UNLIKELY(not first.m_it.primitive_iterator.is_begin() + or not last.m_it.primitive_iterator.is_end())) + { + JSON_THROW(invalid_iterator::create(204, "iterators out of range")); + } + break; + } + + default: + break; + } + + switch (m_type) + { + case value_t::number_integer: + { + m_value.number_integer = first.m_object->m_value.number_integer; + break; + } + + case value_t::number_unsigned: + { + m_value.number_unsigned = first.m_object->m_value.number_unsigned; + break; + } + + case value_t::number_float: + { + m_value.number_float = first.m_object->m_value.number_float; + break; + } + + case value_t::boolean: + { + m_value.boolean = first.m_object->m_value.boolean; + break; + } + + case value_t::string: + { + m_value = *first.m_object->m_value.string; + break; + } + + case value_t::object: + { + m_value.object = create<object_t>(first.m_it.object_iterator, + last.m_it.object_iterator); + break; + } + + case value_t::array: + { + m_value.array = create<array_t>(first.m_it.array_iterator, + last.m_it.array_iterator); + break; + } + + default: + JSON_THROW(invalid_iterator::create(206, "cannot construct with iterators from " + + std::string(first.m_object->type_name()))); + } + + assert_invariant(); + } + + + /////////////////////////////////////// + // other constructors and destructor // + /////////////////////////////////////// + + /// @private + basic_json(const detail::json_ref<basic_json>& ref) + : basic_json(ref.moved_or_copied()) + {} + + /*! + @brief copy constructor + + Creates a copy of a given JSON value. + + @param[in] other the JSON value to copy + + @post `*this == other` + + @complexity Linear in the size of @a other. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes to any JSON value. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is linear. + - As postcondition, it holds: `other == basic_json(other)`. + + @liveexample{The following code shows an example for the copy + constructor.,basic_json__basic_json} + + @since version 1.0.0 + */ + basic_json(const basic_json& other) + : m_type(other.m_type) + { + // check of passed value is valid + other.assert_invariant(); + + switch (m_type) + { + case value_t::object: + { + m_value = *other.m_value.object; + break; + } + + case value_t::array: + { + m_value = *other.m_value.array; + break; + } + + case value_t::string: + { + m_value = *other.m_value.string; + break; + } + + case value_t::boolean: + { + m_value = other.m_value.boolean; + break; + } + + case value_t::number_integer: + { + m_value = other.m_value.number_integer; + break; + } + + case value_t::number_unsigned: + { + m_value = other.m_value.number_unsigned; + break; + } + + case value_t::number_float: + { + m_value = other.m_value.number_float; + break; + } + + default: + break; + } + + assert_invariant(); + } + + /*! + @brief move constructor + + Move constructor. Constructs a JSON value with the contents of the given + value @a other using move semantics. It "steals" the resources from @a + other and leaves it as JSON null value. + + @param[in,out] other value to move to this object + + @post `*this` has the same value as @a other before the call. + @post @a other is a JSON null value. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this constructor never throws + exceptions. + + @requirement This function helps `basic_json` satisfying the + [MoveConstructible](https://en.cppreference.com/w/cpp/named_req/MoveConstructible) + requirements. + + @liveexample{The code below shows the move constructor explicitly called + via std::move.,basic_json__moveconstructor} + + @since version 1.0.0 + */ + basic_json(basic_json&& other) noexcept + : m_type(std::move(other.m_type)), + m_value(std::move(other.m_value)) + { + // check that passed value is valid + other.assert_invariant(); + + // invalidate payload + other.m_type = value_t::null; + other.m_value = {}; + + assert_invariant(); + } + + /*! + @brief copy assignment + + Copy assignment operator. Copies a JSON value via the "copy and swap" + strategy: It is expressed in terms of the copy constructor, destructor, + and the `swap()` member function. + + @param[in] other value to copy from + + @complexity Linear. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is linear. + + @liveexample{The code below shows and example for the copy assignment. It + creates a copy of value `a` which is then swapped with `b`. Finally\, the + copy of `a` (which is the null value after the swap) is + destroyed.,basic_json__copyassignment} + + @since version 1.0.0 + */ + basic_json& operator=(basic_json other) noexcept ( + std::is_nothrow_move_constructible<value_t>::value and + std::is_nothrow_move_assignable<value_t>::value and + std::is_nothrow_move_constructible<json_value>::value and + std::is_nothrow_move_assignable<json_value>::value + ) + { + // check that passed value is valid + other.assert_invariant(); + + using std::swap; + swap(m_type, other.m_type); + swap(m_value, other.m_value); + + assert_invariant(); + return *this; + } + + /*! + @brief destructor + + Destroys the JSON value and frees all allocated memory. + + @complexity Linear. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is linear. + - All stored elements are destroyed and all memory is freed. + + @since version 1.0.0 + */ + ~basic_json() noexcept + { + assert_invariant(); + m_value.destroy(m_type); + } + + /// @} + + public: + /////////////////////// + // object inspection // + /////////////////////// + + /// @name object inspection + /// Functions to inspect the type of a JSON value. + /// @{ + + /*! + @brief serialization + + Serialization function for JSON values. The function tries to mimic + Python's `json.dumps()` function, and currently supports its @a indent + and @a ensure_ascii parameters. + + @param[in] indent If indent is nonnegative, then array elements and object + members will be pretty-printed with that indent level. An indent level of + `0` will only insert newlines. `-1` (the default) selects the most compact + representation. + @param[in] indent_char The character to use for indentation if @a indent is + greater than `0`. The default is ` ` (space). + @param[in] ensure_ascii If @a ensure_ascii is true, all non-ASCII characters + in the output are escaped with `\uXXXX` sequences, and the result consists + of ASCII characters only. + @param[in] error_handler how to react on decoding errors; there are three + possible values: `strict` (throws and exception in case a decoding error + occurs; default), `replace` (replace invalid UTF-8 sequences with U+FFFD), + and `ignore` (ignore invalid UTF-8 sequences during serialization). + + @return string containing the serialization of the JSON value + + @throw type_error.316 if a string stored inside the JSON value is not + UTF-8 encoded + + @complexity Linear. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @liveexample{The following example shows the effect of different @a indent\, + @a indent_char\, and @a ensure_ascii parameters to the result of the + serialization.,dump} + + @see https://docs.python.org/2/library/json.html#json.dump + + @since version 1.0.0; indentation character @a indent_char, option + @a ensure_ascii and exceptions added in version 3.0.0; error + handlers added in version 3.4.0. + */ + string_t dump(const int indent = -1, + const char indent_char = ' ', + const bool ensure_ascii = false, + const error_handler_t error_handler = error_handler_t::strict) const + { + string_t result; + serializer s(detail::output_adapter<char, string_t>(result), indent_char, error_handler); + + if (indent >= 0) + { + s.dump(*this, true, ensure_ascii, static_cast<unsigned int>(indent)); + } + else + { + s.dump(*this, false, ensure_ascii, 0); + } + + return result; + } + + /*! + @brief return the type of the JSON value (explicit) + + Return the type of the JSON value as a value from the @ref value_t + enumeration. + + @return the type of the JSON value + Value type | return value + ------------------------- | ------------------------- + null | value_t::null + boolean | value_t::boolean + string | value_t::string + number (integer) | value_t::number_integer + number (unsigned integer) | value_t::number_unsigned + number (floating-point) | value_t::number_float + object | value_t::object + array | value_t::array + discarded | value_t::discarded + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `type()` for all JSON + types.,type} + + @sa @ref operator value_t() -- return the type of the JSON value (implicit) + @sa @ref type_name() -- return the type as string + + @since version 1.0.0 + */ + constexpr value_t type() const noexcept + { + return m_type; + } + + /*! + @brief return whether type is primitive + + This function returns true if and only if the JSON type is primitive + (string, number, boolean, or null). + + @return `true` if type is primitive (string, number, boolean, or null), + `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_primitive()` for all JSON + types.,is_primitive} + + @sa @ref is_structured() -- returns whether JSON value is structured + @sa @ref is_null() -- returns whether JSON value is `null` + @sa @ref is_string() -- returns whether JSON value is a string + @sa @ref is_boolean() -- returns whether JSON value is a boolean + @sa @ref is_number() -- returns whether JSON value is a number + + @since version 1.0.0 + */ + constexpr bool is_primitive() const noexcept + { + return is_null() or is_string() or is_boolean() or is_number(); + } + + /*! + @brief return whether type is structured + + This function returns true if and only if the JSON type is structured + (array or object). + + @return `true` if type is structured (array or object), `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_structured()` for all JSON + types.,is_structured} + + @sa @ref is_primitive() -- returns whether value is primitive + @sa @ref is_array() -- returns whether value is an array + @sa @ref is_object() -- returns whether value is an object + + @since version 1.0.0 + */ + constexpr bool is_structured() const noexcept + { + return is_array() or is_object(); + } + + /*! + @brief return whether value is null + + This function returns true if and only if the JSON value is null. + + @return `true` if type is null, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_null()` for all JSON + types.,is_null} + + @since version 1.0.0 + */ + constexpr bool is_null() const noexcept + { + return (m_type == value_t::null); + } + + /*! + @brief return whether value is a boolean + + This function returns true if and only if the JSON value is a boolean. + + @return `true` if type is boolean, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_boolean()` for all JSON + types.,is_boolean} + + @since version 1.0.0 + */ + constexpr bool is_boolean() const noexcept + { + return (m_type == value_t::boolean); + } + + /*! + @brief return whether value is a number + + This function returns true if and only if the JSON value is a number. This + includes both integer (signed and unsigned) and floating-point values. + + @return `true` if type is number (regardless whether integer, unsigned + integer or floating-type), `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_number()` for all JSON + types.,is_number} + + @sa @ref is_number_integer() -- check if value is an integer or unsigned + integer number + @sa @ref is_number_unsigned() -- check if value is an unsigned integer + number + @sa @ref is_number_float() -- check if value is a floating-point number + + @since version 1.0.0 + */ + constexpr bool is_number() const noexcept + { + return is_number_integer() or is_number_float(); + } + + /*! + @brief return whether value is an integer number + + This function returns true if and only if the JSON value is a signed or + unsigned integer number. This excludes floating-point values. + + @return `true` if type is an integer or unsigned integer number, `false` + otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_number_integer()` for all + JSON types.,is_number_integer} + + @sa @ref is_number() -- check if value is a number + @sa @ref is_number_unsigned() -- check if value is an unsigned integer + number + @sa @ref is_number_float() -- check if value is a floating-point number + + @since version 1.0.0 + */ + constexpr bool is_number_integer() const noexcept + { + return (m_type == value_t::number_integer or m_type == value_t::number_unsigned); + } + + /*! + @brief return whether value is an unsigned integer number + + This function returns true if and only if the JSON value is an unsigned + integer number. This excludes floating-point and signed integer values. + + @return `true` if type is an unsigned integer number, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_number_unsigned()` for all + JSON types.,is_number_unsigned} + + @sa @ref is_number() -- check if value is a number + @sa @ref is_number_integer() -- check if value is an integer or unsigned + integer number + @sa @ref is_number_float() -- check if value is a floating-point number + + @since version 2.0.0 + */ + constexpr bool is_number_unsigned() const noexcept + { + return (m_type == value_t::number_unsigned); + } + + /*! + @brief return whether value is a floating-point number + + This function returns true if and only if the JSON value is a + floating-point number. This excludes signed and unsigned integer values. + + @return `true` if type is a floating-point number, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_number_float()` for all + JSON types.,is_number_float} + + @sa @ref is_number() -- check if value is number + @sa @ref is_number_integer() -- check if value is an integer number + @sa @ref is_number_unsigned() -- check if value is an unsigned integer + number + + @since version 1.0.0 + */ + constexpr bool is_number_float() const noexcept + { + return (m_type == value_t::number_float); + } + + /*! + @brief return whether value is an object + + This function returns true if and only if the JSON value is an object. + + @return `true` if type is object, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_object()` for all JSON + types.,is_object} + + @since version 1.0.0 + */ + constexpr bool is_object() const noexcept + { + return (m_type == value_t::object); + } + + /*! + @brief return whether value is an array + + This function returns true if and only if the JSON value is an array. + + @return `true` if type is array, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_array()` for all JSON + types.,is_array} + + @since version 1.0.0 + */ + constexpr bool is_array() const noexcept + { + return (m_type == value_t::array); + } + + /*! + @brief return whether value is a string + + This function returns true if and only if the JSON value is a string. + + @return `true` if type is string, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_string()` for all JSON + types.,is_string} + + @since version 1.0.0 + */ + constexpr bool is_string() const noexcept + { + return (m_type == value_t::string); + } + + /*! + @brief return whether value is discarded + + This function returns true if and only if the JSON value was discarded + during parsing with a callback function (see @ref parser_callback_t). + + @note This function will always be `false` for JSON values after parsing. + That is, discarded values can only occur during parsing, but will be + removed when inside a structured value or replaced by null in other cases. + + @return `true` if type is discarded, `false` otherwise. + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies `is_discarded()` for all JSON + types.,is_discarded} + + @since version 1.0.0 + */ + constexpr bool is_discarded() const noexcept + { + return (m_type == value_t::discarded); + } + + /*! + @brief return the type of the JSON value (implicit) + + Implicitly return the type of the JSON value as a value from the @ref + value_t enumeration. + + @return the type of the JSON value + + @complexity Constant. + + @exceptionsafety No-throw guarantee: this member function never throws + exceptions. + + @liveexample{The following code exemplifies the @ref value_t operator for + all JSON types.,operator__value_t} + + @sa @ref type() -- return the type of the JSON value (explicit) + @sa @ref type_name() -- return the type as string + + @since version 1.0.0 + */ + constexpr operator value_t() const noexcept + { + return m_type; + } + + /// @} + + private: + ////////////////// + // value access // + ////////////////// + + /// get a boolean (explicit) + boolean_t get_impl(boolean_t* /*unused*/) const + { + if (JSON_LIKELY(is_boolean())) + { + return m_value.boolean; + } + + JSON_THROW(type_error::create(302, "type must be boolean, but is " + std::string(type_name()))); + } + + /// get a pointer to the value (object) + object_t* get_impl_ptr(object_t* /*unused*/) noexcept + { + return is_object() ? m_value.object : nullptr; + } + + /// get a pointer to the value (object) + constexpr const object_t* get_impl_ptr(const object_t* /*unused*/) const noexcept + { + return is_object() ? m_value.object : nullptr; + } + + /// get a pointer to the value (array) + array_t* get_impl_ptr(array_t* /*unused*/) noexcept + { + return is_array() ? m_value.array : nullptr; + } + + /// get a pointer to the value (array) + constexpr const array_t* get_impl_ptr(const array_t* /*unused*/) const noexcept + { + return is_array() ? m_value.array : nullptr; + } + + /// get a pointer to the value (string) + string_t* get_impl_ptr(string_t* /*unused*/) noexcept + { + return is_string() ? m_value.string : nullptr; + } + + /// get a pointer to the value (string) + constexpr const string_t* get_impl_ptr(const string_t* /*unused*/) const noexcept + { + return is_string() ? m_value.string : nullptr; + } + + /// get a pointer to the value (boolean) + boolean_t* get_impl_ptr(boolean_t* /*unused*/) noexcept + { + return is_boolean() ? &m_value.boolean : nullptr; + } + + /// get a pointer to the value (boolean) + constexpr const boolean_t* get_impl_ptr(const boolean_t* /*unused*/) const noexcept + { + return is_boolean() ? &m_value.boolean : nullptr; + } + + /// get a pointer to the value (integer number) + number_integer_t* get_impl_ptr(number_integer_t* /*unused*/) noexcept + { + return is_number_integer() ? &m_value.number_integer : nullptr; + } + + /// get a pointer to the value (integer number) + constexpr const number_integer_t* get_impl_ptr(const number_integer_t* /*unused*/) const noexcept + { + return is_number_integer() ? &m_value.number_integer : nullptr; + } + + /// get a pointer to the value (unsigned number) + number_unsigned_t* get_impl_ptr(number_unsigned_t* /*unused*/) noexcept + { + return is_number_unsigned() ? &m_value.number_unsigned : nullptr; + } + + /// get a pointer to the value (unsigned number) + constexpr const number_unsigned_t* get_impl_ptr(const number_unsigned_t* /*unused*/) const noexcept + { + return is_number_unsigned() ? &m_value.number_unsigned : nullptr; + } + + /// get a pointer to the value (floating-point number) + number_float_t* get_impl_ptr(number_float_t* /*unused*/) noexcept + { + return is_number_float() ? &m_value.number_float : nullptr; + } + + /// get a pointer to the value (floating-point number) + constexpr const number_float_t* get_impl_ptr(const number_float_t* /*unused*/) const noexcept + { + return is_number_float() ? &m_value.number_float : nullptr; + } + + /*! + @brief helper function to implement get_ref() + + This function helps to implement get_ref() without code duplication for + const and non-const overloads + + @tparam ThisType will be deduced as `basic_json` or `const basic_json` + + @throw type_error.303 if ReferenceType does not match underlying value + type of the current JSON + */ + template<typename ReferenceType, typename ThisType> + static ReferenceType get_ref_impl(ThisType& obj) + { + // delegate the call to get_ptr<>() + auto ptr = obj.template get_ptr<typename std::add_pointer<ReferenceType>::type>(); + + if (JSON_LIKELY(ptr != nullptr)) + { + return *ptr; + } + + JSON_THROW(type_error::create(303, "incompatible ReferenceType for get_ref, actual type is " + std::string(obj.type_name()))); + } + + public: + /// @name value access + /// Direct access to the stored value of a JSON value. + /// @{ + + /*! + @brief get special-case overload + + This overloads avoids a lot of template boilerplate, it can be seen as the + identity method + + @tparam BasicJsonType == @ref basic_json + + @return a copy of *this + + @complexity Constant. + + @since version 2.1.0 + */ + template<typename BasicJsonType, detail::enable_if_t< + std::is_same<typename std::remove_const<BasicJsonType>::type, basic_json_t>::value, + int> = 0> + basic_json get() const + { + return *this; + } + + /*! + @brief get special-case overload + + This overloads converts the current @ref basic_json in a different + @ref basic_json type + + @tparam BasicJsonType == @ref basic_json + + @return a copy of *this, converted into @tparam BasicJsonType + + @complexity Depending on the implementation of the called `from_json()` + method. + + @since version 3.2.0 + */ + template<typename BasicJsonType, detail::enable_if_t< + not std::is_same<BasicJsonType, basic_json>::value and + detail::is_basic_json<BasicJsonType>::value, int> = 0> + BasicJsonType get() const + { + return *this; + } + + /*! + @brief get a value (explicit) + + Explicit type conversion between the JSON value and a compatible value + which is [CopyConstructible](https://en.cppreference.com/w/cpp/named_req/CopyConstructible) + and [DefaultConstructible](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible). + The value is converted by calling the @ref json_serializer<ValueType> + `from_json()` method. + + The function is equivalent to executing + @code {.cpp} + ValueType ret; + JSONSerializer<ValueType>::from_json(*this, ret); + return ret; + @endcode + + This overloads is chosen if: + - @a ValueType is not @ref basic_json, + - @ref json_serializer<ValueType> has a `from_json()` method of the form + `void from_json(const basic_json&, ValueType&)`, and + - @ref json_serializer<ValueType> does not have a `from_json()` method of + the form `ValueType from_json(const basic_json&)` + + @tparam ValueTypeCV the provided value type + @tparam ValueType the returned value type + + @return copy of the JSON value, converted to @a ValueType + + @throw what @ref json_serializer<ValueType> `from_json()` method throws + + @liveexample{The example below shows several conversions from JSON values + to other types. There a few things to note: (1) Floating-point numbers can + be converted to integers\, (2) A JSON array can be converted to a standard + `std::vector<short>`\, (3) A JSON object can be converted to C++ + associative containers such as `std::unordered_map<std::string\, + json>`.,get__ValueType_const} + + @since version 2.1.0 + */ + template<typename ValueTypeCV, typename ValueType = detail::uncvref_t<ValueTypeCV>, + detail::enable_if_t < + not detail::is_basic_json<ValueType>::value and + detail::has_from_json<basic_json_t, ValueType>::value and + not detail::has_non_default_from_json<basic_json_t, ValueType>::value, + int> = 0> + ValueType get() const noexcept(noexcept( + JSONSerializer<ValueType>::from_json(std::declval<const basic_json_t&>(), std::declval<ValueType&>()))) + { + // we cannot static_assert on ValueTypeCV being non-const, because + // there is support for get<const basic_json_t>(), which is why we + // still need the uncvref + static_assert(not std::is_reference<ValueTypeCV>::value, + "get() cannot be used with reference types, you might want to use get_ref()"); + static_assert(std::is_default_constructible<ValueType>::value, + "types must be DefaultConstructible when used with get()"); + + ValueType ret; + JSONSerializer<ValueType>::from_json(*this, ret); + return ret; + } + + /*! + @brief get a value (explicit); special case + + Explicit type conversion between the JSON value and a compatible value + which is **not** [CopyConstructible](https://en.cppreference.com/w/cpp/named_req/CopyConstructible) + and **not** [DefaultConstructible](https://en.cppreference.com/w/cpp/named_req/DefaultConstructible). + The value is converted by calling the @ref json_serializer<ValueType> + `from_json()` method. + + The function is equivalent to executing + @code {.cpp} + return JSONSerializer<ValueTypeCV>::from_json(*this); + @endcode + + This overloads is chosen if: + - @a ValueType is not @ref basic_json and + - @ref json_serializer<ValueType> has a `from_json()` method of the form + `ValueType from_json(const basic_json&)` + + @note If @ref json_serializer<ValueType> has both overloads of + `from_json()`, this one is chosen. + + @tparam ValueTypeCV the provided value type + @tparam ValueType the returned value type + + @return copy of the JSON value, converted to @a ValueType + + @throw what @ref json_serializer<ValueType> `from_json()` method throws + + @since version 2.1.0 + */ + template<typename ValueTypeCV, typename ValueType = detail::uncvref_t<ValueTypeCV>, + detail::enable_if_t<not std::is_same<basic_json_t, ValueType>::value and + detail::has_non_default_from_json<basic_json_t, ValueType>::value, + int> = 0> + ValueType get() const noexcept(noexcept( + JSONSerializer<ValueTypeCV>::from_json(std::declval<const basic_json_t&>()))) + { + static_assert(not std::is_reference<ValueTypeCV>::value, + "get() cannot be used with reference types, you might want to use get_ref()"); + return JSONSerializer<ValueTypeCV>::from_json(*this); + } + + /*! + @brief get a value (explicit) + + Explicit type conversion between the JSON value and a compatible value. + The value is filled into the input parameter by calling the @ref json_serializer<ValueType> + `from_json()` method. + + The function is equivalent to executing + @code {.cpp} + ValueType v; + JSONSerializer<ValueType>::from_json(*this, v); + @endcode + + This overloads is chosen if: + - @a ValueType is not @ref basic_json, + - @ref json_serializer<ValueType> has a `from_json()` method of the form + `void from_json(const basic_json&, ValueType&)`, and + + @tparam ValueType the input parameter type. + + @return the input parameter, allowing chaining calls. + + @throw what @ref json_serializer<ValueType> `from_json()` method throws + + @liveexample{The example below shows several conversions from JSON values + to other types. There a few things to note: (1) Floating-point numbers can + be converted to integers\, (2) A JSON array can be converted to a standard + `std::vector<short>`\, (3) A JSON object can be converted to C++ + associative containers such as `std::unordered_map<std::string\, + json>`.,get_to} + + @since version 3.3.0 + */ + template<typename ValueType, + detail::enable_if_t < + not detail::is_basic_json<ValueType>::value and + detail::has_from_json<basic_json_t, ValueType>::value, + int> = 0> + ValueType & get_to(ValueType& v) const noexcept(noexcept( + JSONSerializer<ValueType>::from_json(std::declval<const basic_json_t&>(), v))) + { + JSONSerializer<ValueType>::from_json(*this, v); + return v; + } + + + /*! + @brief get a pointer value (implicit) + + Implicit pointer access to the internally stored JSON value. No copies are + made. + + @warning Writing data to the pointee of the result yields an undefined + state. + + @tparam PointerType pointer type; must be a pointer to @ref array_t, @ref + object_t, @ref string_t, @ref boolean_t, @ref number_integer_t, + @ref number_unsigned_t, or @ref number_float_t. Enforced by a static + assertion. + + @return pointer to the internally stored JSON value if the requested + pointer type @a PointerType fits to the JSON value; `nullptr` otherwise + + @complexity Constant. + + @liveexample{The example below shows how pointers to internal values of a + JSON value can be requested. Note that no type conversions are made and a + `nullptr` is returned if the value and the requested pointer type does not + match.,get_ptr} + + @since version 1.0.0 + */ + template<typename PointerType, typename std::enable_if< + std::is_pointer<PointerType>::value, int>::type = 0> + auto get_ptr() noexcept -> decltype(std::declval<basic_json_t&>().get_impl_ptr(std::declval<PointerType>())) + { + // delegate the call to get_impl_ptr<>() + return get_impl_ptr(static_cast<PointerType>(nullptr)); + } + + /*! + @brief get a pointer value (implicit) + @copydoc get_ptr() + */ + template<typename PointerType, typename std::enable_if< + std::is_pointer<PointerType>::value and + std::is_const<typename std::remove_pointer<PointerType>::type>::value, int>::type = 0> + constexpr auto get_ptr() const noexcept -> decltype(std::declval<const basic_json_t&>().get_impl_ptr(std::declval<PointerType>())) + { + // delegate the call to get_impl_ptr<>() const + return get_impl_ptr(static_cast<PointerType>(nullptr)); + } + + /*! + @brief get a pointer value (explicit) + + Explicit pointer access to the internally stored JSON value. No copies are + made. + + @warning The pointer becomes invalid if the underlying JSON object + changes. + + @tparam PointerType pointer type; must be a pointer to @ref array_t, @ref + object_t, @ref string_t, @ref boolean_t, @ref number_integer_t, + @ref number_unsigned_t, or @ref number_float_t. + + @return pointer to the internally stored JSON value if the requested + pointer type @a PointerType fits to the JSON value; `nullptr` otherwise + + @complexity Constant. + + @liveexample{The example below shows how pointers to internal values of a + JSON value can be requested. Note that no type conversions are made and a + `nullptr` is returned if the value and the requested pointer type does not + match.,get__PointerType} + + @sa @ref get_ptr() for explicit pointer-member access + + @since version 1.0.0 + */ + template<typename PointerType, typename std::enable_if< + std::is_pointer<PointerType>::value, int>::type = 0> + auto get() noexcept -> decltype(std::declval<basic_json_t&>().template get_ptr<PointerType>()) + { + // delegate the call to get_ptr + return get_ptr<PointerType>(); + } + + /*! + @brief get a pointer value (explicit) + @copydoc get() + */ + template<typename PointerType, typename std::enable_if< + std::is_pointer<PointerType>::value, int>::type = 0> + constexpr auto get() const noexcept -> decltype(std::declval<const basic_json_t&>().template get_ptr<PointerType>()) + { + // delegate the call to get_ptr + return get_ptr<PointerType>(); + } + + /*! + @brief get a reference value (implicit) + + Implicit reference access to the internally stored JSON value. No copies + are made. + + @warning Writing data to the referee of the result yields an undefined + state. + + @tparam ReferenceType reference type; must be a reference to @ref array_t, + @ref object_t, @ref string_t, @ref boolean_t, @ref number_integer_t, or + @ref number_float_t. Enforced by static assertion. + + @return reference to the internally stored JSON value if the requested + reference type @a ReferenceType fits to the JSON value; throws + type_error.303 otherwise + + @throw type_error.303 in case passed type @a ReferenceType is incompatible + with the stored JSON value; see example below + + @complexity Constant. + + @liveexample{The example shows several calls to `get_ref()`.,get_ref} + + @since version 1.1.0 + */ + template<typename ReferenceType, typename std::enable_if< + std::is_reference<ReferenceType>::value, int>::type = 0> + ReferenceType get_ref() + { + // delegate call to get_ref_impl + return get_ref_impl<ReferenceType>(*this); + } + + /*! + @brief get a reference value (implicit) + @copydoc get_ref() + */ + template<typename ReferenceType, typename std::enable_if< + std::is_reference<ReferenceType>::value and + std::is_const<typename std::remove_reference<ReferenceType>::type>::value, int>::type = 0> + ReferenceType get_ref() const + { + // delegate call to get_ref_impl + return get_ref_impl<ReferenceType>(*this); + } + + /*! + @brief get a value (implicit) + + Implicit type conversion between the JSON value and a compatible value. + The call is realized by calling @ref get() const. + + @tparam ValueType non-pointer type compatible to the JSON value, for + instance `int` for JSON integer numbers, `bool` for JSON booleans, or + `std::vector` types for JSON arrays. The character type of @ref string_t + as well as an initializer list of this type is excluded to avoid + ambiguities as these types implicitly convert to `std::string`. + + @return copy of the JSON value, converted to type @a ValueType + + @throw type_error.302 in case passed type @a ValueType is incompatible + to the JSON value type (e.g., the JSON value is of type boolean, but a + string is requested); see example below + + @complexity Linear in the size of the JSON value. + + @liveexample{The example below shows several conversions from JSON values + to other types. There a few things to note: (1) Floating-point numbers can + be converted to integers\, (2) A JSON array can be converted to a standard + `std::vector<short>`\, (3) A JSON object can be converted to C++ + associative containers such as `std::unordered_map<std::string\, + json>`.,operator__ValueType} + + @since version 1.0.0 + */ + template < typename ValueType, typename std::enable_if < + not std::is_pointer<ValueType>::value and + not std::is_same<ValueType, detail::json_ref<basic_json>>::value and + not std::is_same<ValueType, typename string_t::value_type>::value and + not detail::is_basic_json<ValueType>::value + +#ifndef _MSC_VER // fix for issue #167 operator<< ambiguity under VS2015 + and not std::is_same<ValueType, std::initializer_list<typename string_t::value_type>>::value +#if defined(JSON_HAS_CPP_17) && defined(_MSC_VER) and _MSC_VER <= 1914 + and not std::is_same<ValueType, typename std::string_view>::value +#endif +#endif + and detail::is_detected<detail::get_template_function, const basic_json_t&, ValueType>::value + , int >::type = 0 > + operator ValueType() const + { + // delegate the call to get<>() const + return get<ValueType>(); + } + + /// @} + + + //////////////////// + // element access // + //////////////////// + + /// @name element access + /// Access to the JSON value. + /// @{ + + /*! + @brief access specified array element with bounds checking + + Returns a reference to the element at specified location @a idx, with + bounds checking. + + @param[in] idx index of the element to access + + @return reference to the element at index @a idx + + @throw type_error.304 if the JSON value is not an array; in this case, + calling `at` with an index makes no sense. See example below. + @throw out_of_range.401 if the index @a idx is out of range of the array; + that is, `idx >= size()`. See example below. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Constant. + + @since version 1.0.0 + + @liveexample{The example below shows how array elements can be read and + written using `at()`. It also demonstrates the different exceptions that + can be thrown.,at__size_type} + */ + reference at(size_type idx) + { + // at only works for arrays + if (JSON_LIKELY(is_array())) + { + JSON_TRY + { + return m_value.array->at(idx); + } + JSON_CATCH (std::out_of_range&) + { + // create better exception explanation + JSON_THROW(out_of_range::create(401, "array index " + std::to_string(idx) + " is out of range")); + } + } + else + { + JSON_THROW(type_error::create(304, "cannot use at() with " + std::string(type_name()))); + } + } + + /*! + @brief access specified array element with bounds checking + + Returns a const reference to the element at specified location @a idx, + with bounds checking. + + @param[in] idx index of the element to access + + @return const reference to the element at index @a idx + + @throw type_error.304 if the JSON value is not an array; in this case, + calling `at` with an index makes no sense. See example below. + @throw out_of_range.401 if the index @a idx is out of range of the array; + that is, `idx >= size()`. See example below. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Constant. + + @since version 1.0.0 + + @liveexample{The example below shows how array elements can be read using + `at()`. It also demonstrates the different exceptions that can be thrown., + at__size_type_const} + */ + const_reference at(size_type idx) const + { + // at only works for arrays + if (JSON_LIKELY(is_array())) + { + JSON_TRY + { + return m_value.array->at(idx); + } + JSON_CATCH (std::out_of_range&) + { + // create better exception explanation + JSON_THROW(out_of_range::create(401, "array index " + std::to_string(idx) + " is out of range")); + } + } + else + { + JSON_THROW(type_error::create(304, "cannot use at() with " + std::string(type_name()))); + } + } + + /*! + @brief access specified object element with bounds checking + + Returns a reference to the element at with specified key @a key, with + bounds checking. + + @param[in] key key of the element to access + + @return reference to the element at key @a key + + @throw type_error.304 if the JSON value is not an object; in this case, + calling `at` with a key makes no sense. See example below. + @throw out_of_range.403 if the key @a key is is not stored in the object; + that is, `find(key) == end()`. See example below. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Logarithmic in the size of the container. + + @sa @ref operator[](const typename object_t::key_type&) for unchecked + access by reference + @sa @ref value() for access by value with a default value + + @since version 1.0.0 + + @liveexample{The example below shows how object elements can be read and + written using `at()`. It also demonstrates the different exceptions that + can be thrown.,at__object_t_key_type} + */ + reference at(const typename object_t::key_type& key) + { + // at only works for objects + if (JSON_LIKELY(is_object())) + { + JSON_TRY + { + return m_value.object->at(key); + } + JSON_CATCH (std::out_of_range&) + { + // create better exception explanation + JSON_THROW(out_of_range::create(403, "key '" + key + "' not found")); + } + } + else + { + JSON_THROW(type_error::create(304, "cannot use at() with " + std::string(type_name()))); + } + } + + /*! + @brief access specified object element with bounds checking + + Returns a const reference to the element at with specified key @a key, + with bounds checking. + + @param[in] key key of the element to access + + @return const reference to the element at key @a key + + @throw type_error.304 if the JSON value is not an object; in this case, + calling `at` with a key makes no sense. See example below. + @throw out_of_range.403 if the key @a key is is not stored in the object; + that is, `find(key) == end()`. See example below. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Logarithmic in the size of the container. + + @sa @ref operator[](const typename object_t::key_type&) for unchecked + access by reference + @sa @ref value() for access by value with a default value + + @since version 1.0.0 + + @liveexample{The example below shows how object elements can be read using + `at()`. It also demonstrates the different exceptions that can be thrown., + at__object_t_key_type_const} + */ + const_reference at(const typename object_t::key_type& key) const + { + // at only works for objects + if (JSON_LIKELY(is_object())) + { + JSON_TRY + { + return m_value.object->at(key); + } + JSON_CATCH (std::out_of_range&) + { + // create better exception explanation + JSON_THROW(out_of_range::create(403, "key '" + key + "' not found")); + } + } + else + { + JSON_THROW(type_error::create(304, "cannot use at() with " + std::string(type_name()))); + } + } + + /*! + @brief access specified array element + + Returns a reference to the element at specified location @a idx. + + @note If @a idx is beyond the range of the array (i.e., `idx >= size()`), + then the array is silently filled up with `null` values to make `idx` a + valid reference to the last stored element. + + @param[in] idx index of the element to access + + @return reference to the element at index @a idx + + @throw type_error.305 if the JSON value is not an array or null; in that + cases, using the [] operator with an index makes no sense. + + @complexity Constant if @a idx is in the range of the array. Otherwise + linear in `idx - size()`. + + @liveexample{The example below shows how array elements can be read and + written using `[]` operator. Note the addition of `null` + values.,operatorarray__size_type} + + @since version 1.0.0 + */ + reference operator[](size_type idx) + { + // implicitly convert null value to an empty array + if (is_null()) + { + m_type = value_t::array; + m_value.array = create<array_t>(); + assert_invariant(); + } + + // operator[] only works for arrays + if (JSON_LIKELY(is_array())) + { + // fill up array with null values if given idx is outside range + if (idx >= m_value.array->size()) + { + m_value.array->insert(m_value.array->end(), + idx - m_value.array->size() + 1, + basic_json()); + } + + return m_value.array->operator[](idx); + } + + JSON_THROW(type_error::create(305, "cannot use operator[] with a numeric argument with " + std::string(type_name()))); + } + + /*! + @brief access specified array element + + Returns a const reference to the element at specified location @a idx. + + @param[in] idx index of the element to access + + @return const reference to the element at index @a idx + + @throw type_error.305 if the JSON value is not an array; in that case, + using the [] operator with an index makes no sense. + + @complexity Constant. + + @liveexample{The example below shows how array elements can be read using + the `[]` operator.,operatorarray__size_type_const} + + @since version 1.0.0 + */ + const_reference operator[](size_type idx) const + { + // const operator[] only works for arrays + if (JSON_LIKELY(is_array())) + { + return m_value.array->operator[](idx); + } + + JSON_THROW(type_error::create(305, "cannot use operator[] with a numeric argument with " + std::string(type_name()))); + } + + /*! + @brief access specified object element + + Returns a reference to the element at with specified key @a key. + + @note If @a key is not found in the object, then it is silently added to + the object and filled with a `null` value to make `key` a valid reference. + In case the value was `null` before, it is converted to an object. + + @param[in] key key of the element to access + + @return reference to the element at key @a key + + @throw type_error.305 if the JSON value is not an object or null; in that + cases, using the [] operator with a key makes no sense. + + @complexity Logarithmic in the size of the container. + + @liveexample{The example below shows how object elements can be read and + written using the `[]` operator.,operatorarray__key_type} + + @sa @ref at(const typename object_t::key_type&) for access by reference + with range checking + @sa @ref value() for access by value with a default value + + @since version 1.0.0 + */ + reference operator[](const typename object_t::key_type& key) + { + // implicitly convert null value to an empty object + if (is_null()) + { + m_type = value_t::object; + m_value.object = create<object_t>(); + assert_invariant(); + } + + // operator[] only works for objects + if (JSON_LIKELY(is_object())) + { + return m_value.object->operator[](key); + } + + JSON_THROW(type_error::create(305, "cannot use operator[] with a string argument with " + std::string(type_name()))); + } + + /*! + @brief read-only access specified object element + + Returns a const reference to the element at with specified key @a key. No + bounds checking is performed. + + @warning If the element with key @a key does not exist, the behavior is + undefined. + + @param[in] key key of the element to access + + @return const reference to the element at key @a key + + @pre The element with key @a key must exist. **This precondition is + enforced with an assertion.** + + @throw type_error.305 if the JSON value is not an object; in that case, + using the [] operator with a key makes no sense. + + @complexity Logarithmic in the size of the container. + + @liveexample{The example below shows how object elements can be read using + the `[]` operator.,operatorarray__key_type_const} + + @sa @ref at(const typename object_t::key_type&) for access by reference + with range checking + @sa @ref value() for access by value with a default value + + @since version 1.0.0 + */ + const_reference operator[](const typename object_t::key_type& key) const + { + // const operator[] only works for objects + if (JSON_LIKELY(is_object())) + { + assert(m_value.object->find(key) != m_value.object->end()); + return m_value.object->find(key)->second; + } + + JSON_THROW(type_error::create(305, "cannot use operator[] with a string argument with " + std::string(type_name()))); + } + + /*! + @brief access specified object element + + Returns a reference to the element at with specified key @a key. + + @note If @a key is not found in the object, then it is silently added to + the object and filled with a `null` value to make `key` a valid reference. + In case the value was `null` before, it is converted to an object. + + @param[in] key key of the element to access + + @return reference to the element at key @a key + + @throw type_error.305 if the JSON value is not an object or null; in that + cases, using the [] operator with a key makes no sense. + + @complexity Logarithmic in the size of the container. + + @liveexample{The example below shows how object elements can be read and + written using the `[]` operator.,operatorarray__key_type} + + @sa @ref at(const typename object_t::key_type&) for access by reference + with range checking + @sa @ref value() for access by value with a default value + + @since version 1.1.0 + */ + template<typename T> + reference operator[](T* key) + { + // implicitly convert null to object + if (is_null()) + { + m_type = value_t::object; + m_value = value_t::object; + assert_invariant(); + } + + // at only works for objects + if (JSON_LIKELY(is_object())) + { + return m_value.object->operator[](key); + } + + JSON_THROW(type_error::create(305, "cannot use operator[] with a string argument with " + std::string(type_name()))); + } + + /*! + @brief read-only access specified object element + + Returns a const reference to the element at with specified key @a key. No + bounds checking is performed. + + @warning If the element with key @a key does not exist, the behavior is + undefined. + + @param[in] key key of the element to access + + @return const reference to the element at key @a key + + @pre The element with key @a key must exist. **This precondition is + enforced with an assertion.** + + @throw type_error.305 if the JSON value is not an object; in that case, + using the [] operator with a key makes no sense. + + @complexity Logarithmic in the size of the container. + + @liveexample{The example below shows how object elements can be read using + the `[]` operator.,operatorarray__key_type_const} + + @sa @ref at(const typename object_t::key_type&) for access by reference + with range checking + @sa @ref value() for access by value with a default value + + @since version 1.1.0 + */ + template<typename T> + const_reference operator[](T* key) const + { + // at only works for objects + if (JSON_LIKELY(is_object())) + { + assert(m_value.object->find(key) != m_value.object->end()); + return m_value.object->find(key)->second; + } + + JSON_THROW(type_error::create(305, "cannot use operator[] with a string argument with " + std::string(type_name()))); + } + + /*! + @brief access specified object element with default value + + Returns either a copy of an object's element at the specified key @a key + or a given default value if no element with key @a key exists. + + The function is basically equivalent to executing + @code {.cpp} + try { + return at(key); + } catch(out_of_range) { + return default_value; + } + @endcode + + @note Unlike @ref at(const typename object_t::key_type&), this function + does not throw if the given key @a key was not found. + + @note Unlike @ref operator[](const typename object_t::key_type& key), this + function does not implicitly add an element to the position defined by @a + key. This function is furthermore also applicable to const objects. + + @param[in] key key of the element to access + @param[in] default_value the value to return if @a key is not found + + @tparam ValueType type compatible to JSON values, for instance `int` for + JSON integer numbers, `bool` for JSON booleans, or `std::vector` types for + JSON arrays. Note the type of the expected value at @a key and the default + value @a default_value must be compatible. + + @return copy of the element at key @a key or @a default_value if @a key + is not found + + @throw type_error.306 if the JSON value is not an object; in that case, + using `value()` with a key makes no sense. + + @complexity Logarithmic in the size of the container. + + @liveexample{The example below shows how object elements can be queried + with a default value.,basic_json__value} + + @sa @ref at(const typename object_t::key_type&) for access by reference + with range checking + @sa @ref operator[](const typename object_t::key_type&) for unchecked + access by reference + + @since version 1.0.0 + */ + template<class ValueType, typename std::enable_if< + std::is_convertible<basic_json_t, ValueType>::value, int>::type = 0> + ValueType value(const typename object_t::key_type& key, const ValueType& default_value) const + { + // at only works for objects + if (JSON_LIKELY(is_object())) + { + // if key is found, return value and given default value otherwise + const auto it = find(key); + if (it != end()) + { + return *it; + } + + return default_value; + } + + JSON_THROW(type_error::create(306, "cannot use value() with " + std::string(type_name()))); + } + + /*! + @brief overload for a default value of type const char* + @copydoc basic_json::value(const typename object_t::key_type&, const ValueType&) const + */ + string_t value(const typename object_t::key_type& key, const char* default_value) const + { + return value(key, string_t(default_value)); + } + + /*! + @brief access specified object element via JSON Pointer with default value + + Returns either a copy of an object's element at the specified key @a key + or a given default value if no element with key @a key exists. + + The function is basically equivalent to executing + @code {.cpp} + try { + return at(ptr); + } catch(out_of_range) { + return default_value; + } + @endcode + + @note Unlike @ref at(const json_pointer&), this function does not throw + if the given key @a key was not found. + + @param[in] ptr a JSON pointer to the element to access + @param[in] default_value the value to return if @a ptr found no value + + @tparam ValueType type compatible to JSON values, for instance `int` for + JSON integer numbers, `bool` for JSON booleans, or `std::vector` types for + JSON arrays. Note the type of the expected value at @a key and the default + value @a default_value must be compatible. + + @return copy of the element at key @a key or @a default_value if @a key + is not found + + @throw type_error.306 if the JSON value is not an object; in that case, + using `value()` with a key makes no sense. + + @complexity Logarithmic in the size of the container. + + @liveexample{The example below shows how object elements can be queried + with a default value.,basic_json__value_ptr} + + @sa @ref operator[](const json_pointer&) for unchecked access by reference + + @since version 2.0.2 + */ + template<class ValueType, typename std::enable_if< + std::is_convertible<basic_json_t, ValueType>::value, int>::type = 0> + ValueType value(const json_pointer& ptr, const ValueType& default_value) const + { + // at only works for objects + if (JSON_LIKELY(is_object())) + { + // if pointer resolves a value, return it or use default value + JSON_TRY + { + return ptr.get_checked(this); + } + JSON_INTERNAL_CATCH (out_of_range&) + { + return default_value; + } + } + + JSON_THROW(type_error::create(306, "cannot use value() with " + std::string(type_name()))); + } + + /*! + @brief overload for a default value of type const char* + @copydoc basic_json::value(const json_pointer&, ValueType) const + */ + string_t value(const json_pointer& ptr, const char* default_value) const + { + return value(ptr, string_t(default_value)); + } + + /*! + @brief access the first element + + Returns a reference to the first element in the container. For a JSON + container `c`, the expression `c.front()` is equivalent to `*c.begin()`. + + @return In case of a structured type (array or object), a reference to the + first element is returned. In case of number, string, or boolean values, a + reference to the value is returned. + + @complexity Constant. + + @pre The JSON value must not be `null` (would throw `std::out_of_range`) + or an empty array or object (undefined behavior, **guarded by + assertions**). + @post The JSON value remains unchanged. + + @throw invalid_iterator.214 when called on `null` value + + @liveexample{The following code shows an example for `front()`.,front} + + @sa @ref back() -- access the last element + + @since version 1.0.0 + */ + reference front() + { + return *begin(); + } + + /*! + @copydoc basic_json::front() + */ + const_reference front() const + { + return *cbegin(); + } + + /*! + @brief access the last element + + Returns a reference to the last element in the container. For a JSON + container `c`, the expression `c.back()` is equivalent to + @code {.cpp} + auto tmp = c.end(); + --tmp; + return *tmp; + @endcode + + @return In case of a structured type (array or object), a reference to the + last element is returned. In case of number, string, or boolean values, a + reference to the value is returned. + + @complexity Constant. + + @pre The JSON value must not be `null` (would throw `std::out_of_range`) + or an empty array or object (undefined behavior, **guarded by + assertions**). + @post The JSON value remains unchanged. + + @throw invalid_iterator.214 when called on a `null` value. See example + below. + + @liveexample{The following code shows an example for `back()`.,back} + + @sa @ref front() -- access the first element + + @since version 1.0.0 + */ + reference back() + { + auto tmp = end(); + --tmp; + return *tmp; + } + + /*! + @copydoc basic_json::back() + */ + const_reference back() const + { + auto tmp = cend(); + --tmp; + return *tmp; + } + + /*! + @brief remove element given an iterator + + Removes the element specified by iterator @a pos. The iterator @a pos must + be valid and dereferenceable. Thus the `end()` iterator (which is valid, + but is not dereferenceable) cannot be used as a value for @a pos. + + If called on a primitive type other than `null`, the resulting JSON value + will be `null`. + + @param[in] pos iterator to the element to remove + @return Iterator following the last removed element. If the iterator @a + pos refers to the last element, the `end()` iterator is returned. + + @tparam IteratorType an @ref iterator or @ref const_iterator + + @post Invalidates iterators and references at or after the point of the + erase, including the `end()` iterator. + + @throw type_error.307 if called on a `null` value; example: `"cannot use + erase() with null"` + @throw invalid_iterator.202 if called on an iterator which does not belong + to the current JSON value; example: `"iterator does not fit current + value"` + @throw invalid_iterator.205 if called on a primitive type with invalid + iterator (i.e., any iterator which is not `begin()`); example: `"iterator + out of range"` + + @complexity The complexity depends on the type: + - objects: amortized constant + - arrays: linear in distance between @a pos and the end of the container + - strings: linear in the length of the string + - other types: constant + + @liveexample{The example shows the result of `erase()` for different JSON + types.,erase__IteratorType} + + @sa @ref erase(IteratorType, IteratorType) -- removes the elements in + the given range + @sa @ref erase(const typename object_t::key_type&) -- removes the element + from an object at the given key + @sa @ref erase(const size_type) -- removes the element from an array at + the given index + + @since version 1.0.0 + */ + template<class IteratorType, typename std::enable_if< + std::is_same<IteratorType, typename basic_json_t::iterator>::value or + std::is_same<IteratorType, typename basic_json_t::const_iterator>::value, int>::type + = 0> + IteratorType erase(IteratorType pos) + { + // make sure iterator fits the current value + if (JSON_UNLIKELY(this != pos.m_object)) + { + JSON_THROW(invalid_iterator::create(202, "iterator does not fit current value")); + } + + IteratorType result = end(); + + switch (m_type) + { + case value_t::boolean: + case value_t::number_float: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::string: + { + if (JSON_UNLIKELY(not pos.m_it.primitive_iterator.is_begin())) + { + JSON_THROW(invalid_iterator::create(205, "iterator out of range")); + } + + if (is_string()) + { + AllocatorType<string_t> alloc; + std::allocator_traits<decltype(alloc)>::destroy(alloc, m_value.string); + std::allocator_traits<decltype(alloc)>::deallocate(alloc, m_value.string, 1); + m_value.string = nullptr; + } + + m_type = value_t::null; + assert_invariant(); + break; + } + + case value_t::object: + { + result.m_it.object_iterator = m_value.object->erase(pos.m_it.object_iterator); + break; + } + + case value_t::array: + { + result.m_it.array_iterator = m_value.array->erase(pos.m_it.array_iterator); + break; + } + + default: + JSON_THROW(type_error::create(307, "cannot use erase() with " + std::string(type_name()))); + } + + return result; + } + + /*! + @brief remove elements given an iterator range + + Removes the element specified by the range `[first; last)`. The iterator + @a first does not need to be dereferenceable if `first == last`: erasing + an empty range is a no-op. + + If called on a primitive type other than `null`, the resulting JSON value + will be `null`. + + @param[in] first iterator to the beginning of the range to remove + @param[in] last iterator past the end of the range to remove + @return Iterator following the last removed element. If the iterator @a + second refers to the last element, the `end()` iterator is returned. + + @tparam IteratorType an @ref iterator or @ref const_iterator + + @post Invalidates iterators and references at or after the point of the + erase, including the `end()` iterator. + + @throw type_error.307 if called on a `null` value; example: `"cannot use + erase() with null"` + @throw invalid_iterator.203 if called on iterators which does not belong + to the current JSON value; example: `"iterators do not fit current value"` + @throw invalid_iterator.204 if called on a primitive type with invalid + iterators (i.e., if `first != begin()` and `last != end()`); example: + `"iterators out of range"` + + @complexity The complexity depends on the type: + - objects: `log(size()) + std::distance(first, last)` + - arrays: linear in the distance between @a first and @a last, plus linear + in the distance between @a last and end of the container + - strings: linear in the length of the string + - other types: constant + + @liveexample{The example shows the result of `erase()` for different JSON + types.,erase__IteratorType_IteratorType} + + @sa @ref erase(IteratorType) -- removes the element at a given position + @sa @ref erase(const typename object_t::key_type&) -- removes the element + from an object at the given key + @sa @ref erase(const size_type) -- removes the element from an array at + the given index + + @since version 1.0.0 + */ + template<class IteratorType, typename std::enable_if< + std::is_same<IteratorType, typename basic_json_t::iterator>::value or + std::is_same<IteratorType, typename basic_json_t::const_iterator>::value, int>::type + = 0> + IteratorType erase(IteratorType first, IteratorType last) + { + // make sure iterator fits the current value + if (JSON_UNLIKELY(this != first.m_object or this != last.m_object)) + { + JSON_THROW(invalid_iterator::create(203, "iterators do not fit current value")); + } + + IteratorType result = end(); + + switch (m_type) + { + case value_t::boolean: + case value_t::number_float: + case value_t::number_integer: + case value_t::number_unsigned: + case value_t::string: + { + if (JSON_LIKELY(not first.m_it.primitive_iterator.is_begin() + or not last.m_it.primitive_iterator.is_end())) + { + JSON_THROW(invalid_iterator::create(204, "iterators out of range")); + } + + if (is_string()) + { + AllocatorType<string_t> alloc; + std::allocator_traits<decltype(alloc)>::destroy(alloc, m_value.string); + std::allocator_traits<decltype(alloc)>::deallocate(alloc, m_value.string, 1); + m_value.string = nullptr; + } + + m_type = value_t::null; + assert_invariant(); + break; + } + + case value_t::object: + { + result.m_it.object_iterator = m_value.object->erase(first.m_it.object_iterator, + last.m_it.object_iterator); + break; + } + + case value_t::array: + { + result.m_it.array_iterator = m_value.array->erase(first.m_it.array_iterator, + last.m_it.array_iterator); + break; + } + + default: + JSON_THROW(type_error::create(307, "cannot use erase() with " + std::string(type_name()))); + } + + return result; + } + + /*! + @brief remove element from a JSON object given a key + + Removes elements from a JSON object with the key value @a key. + + @param[in] key value of the elements to remove + + @return Number of elements removed. If @a ObjectType is the default + `std::map` type, the return value will always be `0` (@a key was not + found) or `1` (@a key was found). + + @post References and iterators to the erased elements are invalidated. + Other references and iterators are not affected. + + @throw type_error.307 when called on a type other than JSON object; + example: `"cannot use erase() with null"` + + @complexity `log(size()) + count(key)` + + @liveexample{The example shows the effect of `erase()`.,erase__key_type} + + @sa @ref erase(IteratorType) -- removes the element at a given position + @sa @ref erase(IteratorType, IteratorType) -- removes the elements in + the given range + @sa @ref erase(const size_type) -- removes the element from an array at + the given index + + @since version 1.0.0 + */ + size_type erase(const typename object_t::key_type& key) + { + // this erase only works for objects + if (JSON_LIKELY(is_object())) + { + return m_value.object->erase(key); + } + + JSON_THROW(type_error::create(307, "cannot use erase() with " + std::string(type_name()))); + } + + /*! + @brief remove element from a JSON array given an index + + Removes element from a JSON array at the index @a idx. + + @param[in] idx index of the element to remove + + @throw type_error.307 when called on a type other than JSON object; + example: `"cannot use erase() with null"` + @throw out_of_range.401 when `idx >= size()`; example: `"array index 17 + is out of range"` + + @complexity Linear in distance between @a idx and the end of the container. + + @liveexample{The example shows the effect of `erase()`.,erase__size_type} + + @sa @ref erase(IteratorType) -- removes the element at a given position + @sa @ref erase(IteratorType, IteratorType) -- removes the elements in + the given range + @sa @ref erase(const typename object_t::key_type&) -- removes the element + from an object at the given key + + @since version 1.0.0 + */ + void erase(const size_type idx) + { + // this erase only works for arrays + if (JSON_LIKELY(is_array())) + { + if (JSON_UNLIKELY(idx >= size())) + { + JSON_THROW(out_of_range::create(401, "array index " + std::to_string(idx) + " is out of range")); + } + + m_value.array->erase(m_value.array->begin() + static_cast<difference_type>(idx)); + } + else + { + JSON_THROW(type_error::create(307, "cannot use erase() with " + std::string(type_name()))); + } + } + + /// @} + + + //////////// + // lookup // + //////////// + + /// @name lookup + /// @{ + + /*! + @brief find an element in a JSON object + + Finds an element in a JSON object with key equivalent to @a key. If the + element is not found or the JSON value is not an object, end() is + returned. + + @note This method always returns @ref end() when executed on a JSON type + that is not an object. + + @param[in] key key value of the element to search for. + + @return Iterator to an element with key equivalent to @a key. If no such + element is found or the JSON value is not an object, past-the-end (see + @ref end()) iterator is returned. + + @complexity Logarithmic in the size of the JSON object. + + @liveexample{The example shows how `find()` is used.,find__key_type} + + @since version 1.0.0 + */ + template<typename KeyT> + iterator find(KeyT&& key) + { + auto result = end(); + + if (is_object()) + { + result.m_it.object_iterator = m_value.object->find(std::forward<KeyT>(key)); + } + + return result; + } + + /*! + @brief find an element in a JSON object + @copydoc find(KeyT&&) + */ + template<typename KeyT> + const_iterator find(KeyT&& key) const + { + auto result = cend(); + + if (is_object()) + { + result.m_it.object_iterator = m_value.object->find(std::forward<KeyT>(key)); + } + + return result; + } + + /*! + @brief returns the number of occurrences of a key in a JSON object + + Returns the number of elements with key @a key. If ObjectType is the + default `std::map` type, the return value will always be `0` (@a key was + not found) or `1` (@a key was found). + + @note This method always returns `0` when executed on a JSON type that is + not an object. + + @param[in] key key value of the element to count + + @return Number of elements with key @a key. If the JSON value is not an + object, the return value will be `0`. + + @complexity Logarithmic in the size of the JSON object. + + @liveexample{The example shows how `count()` is used.,count} + + @since version 1.0.0 + */ + template<typename KeyT> + size_type count(KeyT&& key) const + { + // return 0 for all nonobject types + return is_object() ? m_value.object->count(std::forward<KeyT>(key)) : 0; + } + + /// @} + + + /////////////// + // iterators // + /////////////// + + /// @name iterators + /// @{ + + /*! + @brief returns an iterator to the first element + + Returns an iterator to the first element. + + @image html range-begin-end.svg "Illustration from cppreference.com" + + @return iterator to the first element + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + + @liveexample{The following code shows an example for `begin()`.,begin} + + @sa @ref cbegin() -- returns a const iterator to the beginning + @sa @ref end() -- returns an iterator to the end + @sa @ref cend() -- returns a const iterator to the end + + @since version 1.0.0 + */ + iterator begin() noexcept + { + iterator result(this); + result.set_begin(); + return result; + } + + /*! + @copydoc basic_json::cbegin() + */ + const_iterator begin() const noexcept + { + return cbegin(); + } + + /*! + @brief returns a const iterator to the first element + + Returns a const iterator to the first element. + + @image html range-begin-end.svg "Illustration from cppreference.com" + + @return const iterator to the first element + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + - Has the semantics of `const_cast<const basic_json&>(*this).begin()`. + + @liveexample{The following code shows an example for `cbegin()`.,cbegin} + + @sa @ref begin() -- returns an iterator to the beginning + @sa @ref end() -- returns an iterator to the end + @sa @ref cend() -- returns a const iterator to the end + + @since version 1.0.0 + */ + const_iterator cbegin() const noexcept + { + const_iterator result(this); + result.set_begin(); + return result; + } + + /*! + @brief returns an iterator to one past the last element + + Returns an iterator to one past the last element. + + @image html range-begin-end.svg "Illustration from cppreference.com" + + @return iterator one past the last element + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + + @liveexample{The following code shows an example for `end()`.,end} + + @sa @ref cend() -- returns a const iterator to the end + @sa @ref begin() -- returns an iterator to the beginning + @sa @ref cbegin() -- returns a const iterator to the beginning + + @since version 1.0.0 + */ + iterator end() noexcept + { + iterator result(this); + result.set_end(); + return result; + } + + /*! + @copydoc basic_json::cend() + */ + const_iterator end() const noexcept + { + return cend(); + } + + /*! + @brief returns a const iterator to one past the last element + + Returns a const iterator to one past the last element. + + @image html range-begin-end.svg "Illustration from cppreference.com" + + @return const iterator one past the last element + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + - Has the semantics of `const_cast<const basic_json&>(*this).end()`. + + @liveexample{The following code shows an example for `cend()`.,cend} + + @sa @ref end() -- returns an iterator to the end + @sa @ref begin() -- returns an iterator to the beginning + @sa @ref cbegin() -- returns a const iterator to the beginning + + @since version 1.0.0 + */ + const_iterator cend() const noexcept + { + const_iterator result(this); + result.set_end(); + return result; + } + + /*! + @brief returns an iterator to the reverse-beginning + + Returns an iterator to the reverse-beginning; that is, the last element. + + @image html range-rbegin-rend.svg "Illustration from cppreference.com" + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [ReversibleContainer](https://en.cppreference.com/w/cpp/named_req/ReversibleContainer) + requirements: + - The complexity is constant. + - Has the semantics of `reverse_iterator(end())`. + + @liveexample{The following code shows an example for `rbegin()`.,rbegin} + + @sa @ref crbegin() -- returns a const reverse iterator to the beginning + @sa @ref rend() -- returns a reverse iterator to the end + @sa @ref crend() -- returns a const reverse iterator to the end + + @since version 1.0.0 + */ + reverse_iterator rbegin() noexcept + { + return reverse_iterator(end()); + } + + /*! + @copydoc basic_json::crbegin() + */ + const_reverse_iterator rbegin() const noexcept + { + return crbegin(); + } + + /*! + @brief returns an iterator to the reverse-end + + Returns an iterator to the reverse-end; that is, one before the first + element. + + @image html range-rbegin-rend.svg "Illustration from cppreference.com" + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [ReversibleContainer](https://en.cppreference.com/w/cpp/named_req/ReversibleContainer) + requirements: + - The complexity is constant. + - Has the semantics of `reverse_iterator(begin())`. + + @liveexample{The following code shows an example for `rend()`.,rend} + + @sa @ref crend() -- returns a const reverse iterator to the end + @sa @ref rbegin() -- returns a reverse iterator to the beginning + @sa @ref crbegin() -- returns a const reverse iterator to the beginning + + @since version 1.0.0 + */ + reverse_iterator rend() noexcept + { + return reverse_iterator(begin()); + } + + /*! + @copydoc basic_json::crend() + */ + const_reverse_iterator rend() const noexcept + { + return crend(); + } + + /*! + @brief returns a const reverse iterator to the last element + + Returns a const iterator to the reverse-beginning; that is, the last + element. + + @image html range-rbegin-rend.svg "Illustration from cppreference.com" + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [ReversibleContainer](https://en.cppreference.com/w/cpp/named_req/ReversibleContainer) + requirements: + - The complexity is constant. + - Has the semantics of `const_cast<const basic_json&>(*this).rbegin()`. + + @liveexample{The following code shows an example for `crbegin()`.,crbegin} + + @sa @ref rbegin() -- returns a reverse iterator to the beginning + @sa @ref rend() -- returns a reverse iterator to the end + @sa @ref crend() -- returns a const reverse iterator to the end + + @since version 1.0.0 + */ + const_reverse_iterator crbegin() const noexcept + { + return const_reverse_iterator(cend()); + } + + /*! + @brief returns a const reverse iterator to one before the first + + Returns a const reverse iterator to the reverse-end; that is, one before + the first element. + + @image html range-rbegin-rend.svg "Illustration from cppreference.com" + + @complexity Constant. + + @requirement This function helps `basic_json` satisfying the + [ReversibleContainer](https://en.cppreference.com/w/cpp/named_req/ReversibleContainer) + requirements: + - The complexity is constant. + - Has the semantics of `const_cast<const basic_json&>(*this).rend()`. + + @liveexample{The following code shows an example for `crend()`.,crend} + + @sa @ref rend() -- returns a reverse iterator to the end + @sa @ref rbegin() -- returns a reverse iterator to the beginning + @sa @ref crbegin() -- returns a const reverse iterator to the beginning + + @since version 1.0.0 + */ + const_reverse_iterator crend() const noexcept + { + return const_reverse_iterator(cbegin()); + } + + public: + /*! + @brief wrapper to access iterator member functions in range-based for + + This function allows to access @ref iterator::key() and @ref + iterator::value() during range-based for loops. In these loops, a + reference to the JSON values is returned, so there is no access to the + underlying iterator. + + For loop without iterator_wrapper: + + @code{cpp} + for (auto it = j_object.begin(); it != j_object.end(); ++it) + { + std::cout << "key: " << it.key() << ", value:" << it.value() << '\n'; + } + @endcode + + Range-based for loop without iterator proxy: + + @code{cpp} + for (auto it : j_object) + { + // "it" is of type json::reference and has no key() member + std::cout << "value: " << it << '\n'; + } + @endcode + + Range-based for loop with iterator proxy: + + @code{cpp} + for (auto it : json::iterator_wrapper(j_object)) + { + std::cout << "key: " << it.key() << ", value:" << it.value() << '\n'; + } + @endcode + + @note When iterating over an array, `key()` will return the index of the + element as string (see example). + + @param[in] ref reference to a JSON value + @return iteration proxy object wrapping @a ref with an interface to use in + range-based for loops + + @liveexample{The following code shows how the wrapper is used,iterator_wrapper} + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Constant. + + @note The name of this function is not yet final and may change in the + future. + + @deprecated This stream operator is deprecated and will be removed in + future 4.0.0 of the library. Please use @ref items() instead; + that is, replace `json::iterator_wrapper(j)` with `j.items()`. + */ + JSON_DEPRECATED + static iteration_proxy<iterator> iterator_wrapper(reference ref) noexcept + { + return ref.items(); + } + + /*! + @copydoc iterator_wrapper(reference) + */ + JSON_DEPRECATED + static iteration_proxy<const_iterator> iterator_wrapper(const_reference ref) noexcept + { + return ref.items(); + } + + /*! + @brief helper to access iterator member functions in range-based for + + This function allows to access @ref iterator::key() and @ref + iterator::value() during range-based for loops. In these loops, a + reference to the JSON values is returned, so there is no access to the + underlying iterator. + + For loop without `items()` function: + + @code{cpp} + for (auto it = j_object.begin(); it != j_object.end(); ++it) + { + std::cout << "key: " << it.key() << ", value:" << it.value() << '\n'; + } + @endcode + + Range-based for loop without `items()` function: + + @code{cpp} + for (auto it : j_object) + { + // "it" is of type json::reference and has no key() member + std::cout << "value: " << it << '\n'; + } + @endcode + + Range-based for loop with `items()` function: + + @code{cpp} + for (auto it : j_object.items()) + { + std::cout << "key: " << it.key() << ", value:" << it.value() << '\n'; + } + @endcode + + @note When iterating over an array, `key()` will return the index of the + element as string (see example). For primitive types (e.g., numbers), + `key()` returns an empty string. + + @return iteration proxy object wrapping @a ref with an interface to use in + range-based for loops + + @liveexample{The following code shows how the function is used.,items} + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Constant. + + @since version 3.1.0. + */ + iteration_proxy<iterator> items() noexcept + { + return iteration_proxy<iterator>(*this); + } + + /*! + @copydoc items() + */ + iteration_proxy<const_iterator> items() const noexcept + { + return iteration_proxy<const_iterator>(*this); + } + + /// @} + + + ////////////// + // capacity // + ////////////// + + /// @name capacity + /// @{ + + /*! + @brief checks whether the container is empty. + + Checks if a JSON value has no elements (i.e. whether its @ref size is `0`). + + @return The return value depends on the different types and is + defined as follows: + Value type | return value + ----------- | ------------- + null | `true` + boolean | `false` + string | `false` + number | `false` + object | result of function `object_t::empty()` + array | result of function `array_t::empty()` + + @liveexample{The following code uses `empty()` to check if a JSON + object contains any elements.,empty} + + @complexity Constant, as long as @ref array_t and @ref object_t satisfy + the Container concept; that is, their `empty()` functions have constant + complexity. + + @iterators No changes. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @note This function does not return whether a string stored as JSON value + is empty - it returns whether the JSON container itself is empty which is + false in the case of a string. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + - Has the semantics of `begin() == end()`. + + @sa @ref size() -- returns the number of elements + + @since version 1.0.0 + */ + bool empty() const noexcept + { + switch (m_type) + { + case value_t::null: + { + // null values are empty + return true; + } + + case value_t::array: + { + // delegate call to array_t::empty() + return m_value.array->empty(); + } + + case value_t::object: + { + // delegate call to object_t::empty() + return m_value.object->empty(); + } + + default: + { + // all other types are nonempty + return false; + } + } + } + + /*! + @brief returns the number of elements + + Returns the number of elements in a JSON value. + + @return The return value depends on the different types and is + defined as follows: + Value type | return value + ----------- | ------------- + null | `0` + boolean | `1` + string | `1` + number | `1` + object | result of function object_t::size() + array | result of function array_t::size() + + @liveexample{The following code calls `size()` on the different value + types.,size} + + @complexity Constant, as long as @ref array_t and @ref object_t satisfy + the Container concept; that is, their size() functions have constant + complexity. + + @iterators No changes. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @note This function does not return the length of a string stored as JSON + value - it returns the number of elements in the JSON value which is 1 in + the case of a string. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + - Has the semantics of `std::distance(begin(), end())`. + + @sa @ref empty() -- checks whether the container is empty + @sa @ref max_size() -- returns the maximal number of elements + + @since version 1.0.0 + */ + size_type size() const noexcept + { + switch (m_type) + { + case value_t::null: + { + // null values are empty + return 0; + } + + case value_t::array: + { + // delegate call to array_t::size() + return m_value.array->size(); + } + + case value_t::object: + { + // delegate call to object_t::size() + return m_value.object->size(); + } + + default: + { + // all other types have size 1 + return 1; + } + } + } + + /*! + @brief returns the maximum possible number of elements + + Returns the maximum number of elements a JSON value is able to hold due to + system or library implementation limitations, i.e. `std::distance(begin(), + end())` for the JSON value. + + @return The return value depends on the different types and is + defined as follows: + Value type | return value + ----------- | ------------- + null | `0` (same as `size()`) + boolean | `1` (same as `size()`) + string | `1` (same as `size()`) + number | `1` (same as `size()`) + object | result of function `object_t::max_size()` + array | result of function `array_t::max_size()` + + @liveexample{The following code calls `max_size()` on the different value + types. Note the output is implementation specific.,max_size} + + @complexity Constant, as long as @ref array_t and @ref object_t satisfy + the Container concept; that is, their `max_size()` functions have constant + complexity. + + @iterators No changes. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @requirement This function helps `basic_json` satisfying the + [Container](https://en.cppreference.com/w/cpp/named_req/Container) + requirements: + - The complexity is constant. + - Has the semantics of returning `b.size()` where `b` is the largest + possible JSON value. + + @sa @ref size() -- returns the number of elements + + @since version 1.0.0 + */ + size_type max_size() const noexcept + { + switch (m_type) + { + case value_t::array: + { + // delegate call to array_t::max_size() + return m_value.array->max_size(); + } + + case value_t::object: + { + // delegate call to object_t::max_size() + return m_value.object->max_size(); + } + + default: + { + // all other types have max_size() == size() + return size(); + } + } + } + + /// @} + + + /////////////// + // modifiers // + /////////////// + + /// @name modifiers + /// @{ + + /*! + @brief clears the contents + + Clears the content of a JSON value and resets it to the default value as + if @ref basic_json(value_t) would have been called with the current value + type from @ref type(): + + Value type | initial value + ----------- | ------------- + null | `null` + boolean | `false` + string | `""` + number | `0` + object | `{}` + array | `[]` + + @post Has the same effect as calling + @code {.cpp} + *this = basic_json(type()); + @endcode + + @liveexample{The example below shows the effect of `clear()` to different + JSON types.,clear} + + @complexity Linear in the size of the JSON value. + + @iterators All iterators, pointers and references related to this container + are invalidated. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @sa @ref basic_json(value_t) -- constructor that creates an object with the + same value than calling `clear()` + + @since version 1.0.0 + */ + void clear() noexcept + { + switch (m_type) + { + case value_t::number_integer: + { + m_value.number_integer = 0; + break; + } + + case value_t::number_unsigned: + { + m_value.number_unsigned = 0; + break; + } + + case value_t::number_float: + { + m_value.number_float = 0.0; + break; + } + + case value_t::boolean: + { + m_value.boolean = false; + break; + } + + case value_t::string: + { + m_value.string->clear(); + break; + } + + case value_t::array: + { + m_value.array->clear(); + break; + } + + case value_t::object: + { + m_value.object->clear(); + break; + } + + default: + break; + } + } + + /*! + @brief add an object to an array + + Appends the given element @a val to the end of the JSON value. If the + function is called on a JSON null value, an empty array is created before + appending @a val. + + @param[in] val the value to add to the JSON array + + @throw type_error.308 when called on a type other than JSON array or + null; example: `"cannot use push_back() with number"` + + @complexity Amortized constant. + + @liveexample{The example shows how `push_back()` and `+=` can be used to + add elements to a JSON array. Note how the `null` value was silently + converted to a JSON array.,push_back} + + @since version 1.0.0 + */ + void push_back(basic_json&& val) + { + // push_back only works for null objects or arrays + if (JSON_UNLIKELY(not(is_null() or is_array()))) + { + JSON_THROW(type_error::create(308, "cannot use push_back() with " + std::string(type_name()))); + } + + // transform null object into an array + if (is_null()) + { + m_type = value_t::array; + m_value = value_t::array; + assert_invariant(); + } + + // add element to array (move semantics) + m_value.array->push_back(std::move(val)); + // invalidate object + val.m_type = value_t::null; + } + + /*! + @brief add an object to an array + @copydoc push_back(basic_json&&) + */ + reference operator+=(basic_json&& val) + { + push_back(std::move(val)); + return *this; + } + + /*! + @brief add an object to an array + @copydoc push_back(basic_json&&) + */ + void push_back(const basic_json& val) + { + // push_back only works for null objects or arrays + if (JSON_UNLIKELY(not(is_null() or is_array()))) + { + JSON_THROW(type_error::create(308, "cannot use push_back() with " + std::string(type_name()))); + } + + // transform null object into an array + if (is_null()) + { + m_type = value_t::array; + m_value = value_t::array; + assert_invariant(); + } + + // add element to array + m_value.array->push_back(val); + } + + /*! + @brief add an object to an array + @copydoc push_back(basic_json&&) + */ + reference operator+=(const basic_json& val) + { + push_back(val); + return *this; + } + + /*! + @brief add an object to an object + + Inserts the given element @a val to the JSON object. If the function is + called on a JSON null value, an empty object is created before inserting + @a val. + + @param[in] val the value to add to the JSON object + + @throw type_error.308 when called on a type other than JSON object or + null; example: `"cannot use push_back() with number"` + + @complexity Logarithmic in the size of the container, O(log(`size()`)). + + @liveexample{The example shows how `push_back()` and `+=` can be used to + add elements to a JSON object. Note how the `null` value was silently + converted to a JSON object.,push_back__object_t__value} + + @since version 1.0.0 + */ + void push_back(const typename object_t::value_type& val) + { + // push_back only works for null objects or objects + if (JSON_UNLIKELY(not(is_null() or is_object()))) + { + JSON_THROW(type_error::create(308, "cannot use push_back() with " + std::string(type_name()))); + } + + // transform null object into an object + if (is_null()) + { + m_type = value_t::object; + m_value = value_t::object; + assert_invariant(); + } + + // add element to array + m_value.object->insert(val); + } + + /*! + @brief add an object to an object + @copydoc push_back(const typename object_t::value_type&) + */ + reference operator+=(const typename object_t::value_type& val) + { + push_back(val); + return *this; + } + + /*! + @brief add an object to an object + + This function allows to use `push_back` with an initializer list. In case + + 1. the current value is an object, + 2. the initializer list @a init contains only two elements, and + 3. the first element of @a init is a string, + + @a init is converted into an object element and added using + @ref push_back(const typename object_t::value_type&). Otherwise, @a init + is converted to a JSON value and added using @ref push_back(basic_json&&). + + @param[in] init an initializer list + + @complexity Linear in the size of the initializer list @a init. + + @note This function is required to resolve an ambiguous overload error, + because pairs like `{"key", "value"}` can be both interpreted as + `object_t::value_type` or `std::initializer_list<basic_json>`, see + https://github.com/nlohmann/json/issues/235 for more information. + + @liveexample{The example shows how initializer lists are treated as + objects when possible.,push_back__initializer_list} + */ + void push_back(initializer_list_t init) + { + if (is_object() and init.size() == 2 and (*init.begin())->is_string()) + { + basic_json&& key = init.begin()->moved_or_copied(); + push_back(typename object_t::value_type( + std::move(key.get_ref<string_t&>()), (init.begin() + 1)->moved_or_copied())); + } + else + { + push_back(basic_json(init)); + } + } + + /*! + @brief add an object to an object + @copydoc push_back(initializer_list_t) + */ + reference operator+=(initializer_list_t init) + { + push_back(init); + return *this; + } + + /*! + @brief add an object to an array + + Creates a JSON value from the passed parameters @a args to the end of the + JSON value. If the function is called on a JSON null value, an empty array + is created before appending the value created from @a args. + + @param[in] args arguments to forward to a constructor of @ref basic_json + @tparam Args compatible types to create a @ref basic_json object + + @throw type_error.311 when called on a type other than JSON array or + null; example: `"cannot use emplace_back() with number"` + + @complexity Amortized constant. + + @liveexample{The example shows how `push_back()` can be used to add + elements to a JSON array. Note how the `null` value was silently converted + to a JSON array.,emplace_back} + + @since version 2.0.8 + */ + template<class... Args> + void emplace_back(Args&& ... args) + { + // emplace_back only works for null objects or arrays + if (JSON_UNLIKELY(not(is_null() or is_array()))) + { + JSON_THROW(type_error::create(311, "cannot use emplace_back() with " + std::string(type_name()))); + } + + // transform null object into an array + if (is_null()) + { + m_type = value_t::array; + m_value = value_t::array; + assert_invariant(); + } + + // add element to array (perfect forwarding) + m_value.array->emplace_back(std::forward<Args>(args)...); + } + + /*! + @brief add an object to an object if key does not exist + + Inserts a new element into a JSON object constructed in-place with the + given @a args if there is no element with the key in the container. If the + function is called on a JSON null value, an empty object is created before + appending the value created from @a args. + + @param[in] args arguments to forward to a constructor of @ref basic_json + @tparam Args compatible types to create a @ref basic_json object + + @return a pair consisting of an iterator to the inserted element, or the + already-existing element if no insertion happened, and a bool + denoting whether the insertion took place. + + @throw type_error.311 when called on a type other than JSON object or + null; example: `"cannot use emplace() with number"` + + @complexity Logarithmic in the size of the container, O(log(`size()`)). + + @liveexample{The example shows how `emplace()` can be used to add elements + to a JSON object. Note how the `null` value was silently converted to a + JSON object. Further note how no value is added if there was already one + value stored with the same key.,emplace} + + @since version 2.0.8 + */ + template<class... Args> + std::pair<iterator, bool> emplace(Args&& ... args) + { + // emplace only works for null objects or arrays + if (JSON_UNLIKELY(not(is_null() or is_object()))) + { + JSON_THROW(type_error::create(311, "cannot use emplace() with " + std::string(type_name()))); + } + + // transform null object into an object + if (is_null()) + { + m_type = value_t::object; + m_value = value_t::object; + assert_invariant(); + } + + // add element to array (perfect forwarding) + auto res = m_value.object->emplace(std::forward<Args>(args)...); + // create result iterator and set iterator to the result of emplace + auto it = begin(); + it.m_it.object_iterator = res.first; + + // return pair of iterator and boolean + return {it, res.second}; + } + + /// Helper for insertion of an iterator + /// @note: This uses std::distance to support GCC 4.8, + /// see https://github.com/nlohmann/json/pull/1257 + template<typename... Args> + iterator insert_iterator(const_iterator pos, Args&& ... args) + { + iterator result(this); + assert(m_value.array != nullptr); + + auto insert_pos = std::distance(m_value.array->begin(), pos.m_it.array_iterator); + m_value.array->insert(pos.m_it.array_iterator, std::forward<Args>(args)...); + result.m_it.array_iterator = m_value.array->begin() + insert_pos; + + // This could have been written as: + // result.m_it.array_iterator = m_value.array->insert(pos.m_it.array_iterator, cnt, val); + // but the return value of insert is missing in GCC 4.8, so it is written this way instead. + + return result; + } + + /*! + @brief inserts element + + Inserts element @a val before iterator @a pos. + + @param[in] pos iterator before which the content will be inserted; may be + the end() iterator + @param[in] val element to insert + @return iterator pointing to the inserted @a val. + + @throw type_error.309 if called on JSON values other than arrays; + example: `"cannot use insert() with string"` + @throw invalid_iterator.202 if @a pos is not an iterator of *this; + example: `"iterator does not fit current value"` + + @complexity Constant plus linear in the distance between @a pos and end of + the container. + + @liveexample{The example shows how `insert()` is used.,insert} + + @since version 1.0.0 + */ + iterator insert(const_iterator pos, const basic_json& val) + { + // insert only works for arrays + if (JSON_LIKELY(is_array())) + { + // check if iterator pos fits to this JSON value + if (JSON_UNLIKELY(pos.m_object != this)) + { + JSON_THROW(invalid_iterator::create(202, "iterator does not fit current value")); + } + + // insert to array and return iterator + return insert_iterator(pos, val); + } + + JSON_THROW(type_error::create(309, "cannot use insert() with " + std::string(type_name()))); + } + + /*! + @brief inserts element + @copydoc insert(const_iterator, const basic_json&) + */ + iterator insert(const_iterator pos, basic_json&& val) + { + return insert(pos, val); + } + + /*! + @brief inserts elements + + Inserts @a cnt copies of @a val before iterator @a pos. + + @param[in] pos iterator before which the content will be inserted; may be + the end() iterator + @param[in] cnt number of copies of @a val to insert + @param[in] val element to insert + @return iterator pointing to the first element inserted, or @a pos if + `cnt==0` + + @throw type_error.309 if called on JSON values other than arrays; example: + `"cannot use insert() with string"` + @throw invalid_iterator.202 if @a pos is not an iterator of *this; + example: `"iterator does not fit current value"` + + @complexity Linear in @a cnt plus linear in the distance between @a pos + and end of the container. + + @liveexample{The example shows how `insert()` is used.,insert__count} + + @since version 1.0.0 + */ + iterator insert(const_iterator pos, size_type cnt, const basic_json& val) + { + // insert only works for arrays + if (JSON_LIKELY(is_array())) + { + // check if iterator pos fits to this JSON value + if (JSON_UNLIKELY(pos.m_object != this)) + { + JSON_THROW(invalid_iterator::create(202, "iterator does not fit current value")); + } + + // insert to array and return iterator + return insert_iterator(pos, cnt, val); + } + + JSON_THROW(type_error::create(309, "cannot use insert() with " + std::string(type_name()))); + } + + /*! + @brief inserts elements + + Inserts elements from range `[first, last)` before iterator @a pos. + + @param[in] pos iterator before which the content will be inserted; may be + the end() iterator + @param[in] first begin of the range of elements to insert + @param[in] last end of the range of elements to insert + + @throw type_error.309 if called on JSON values other than arrays; example: + `"cannot use insert() with string"` + @throw invalid_iterator.202 if @a pos is not an iterator of *this; + example: `"iterator does not fit current value"` + @throw invalid_iterator.210 if @a first and @a last do not belong to the + same JSON value; example: `"iterators do not fit"` + @throw invalid_iterator.211 if @a first or @a last are iterators into + container for which insert is called; example: `"passed iterators may not + belong to container"` + + @return iterator pointing to the first element inserted, or @a pos if + `first==last` + + @complexity Linear in `std::distance(first, last)` plus linear in the + distance between @a pos and end of the container. + + @liveexample{The example shows how `insert()` is used.,insert__range} + + @since version 1.0.0 + */ + iterator insert(const_iterator pos, const_iterator first, const_iterator last) + { + // insert only works for arrays + if (JSON_UNLIKELY(not is_array())) + { + JSON_THROW(type_error::create(309, "cannot use insert() with " + std::string(type_name()))); + } + + // check if iterator pos fits to this JSON value + if (JSON_UNLIKELY(pos.m_object != this)) + { + JSON_THROW(invalid_iterator::create(202, "iterator does not fit current value")); + } + + // check if range iterators belong to the same JSON object + if (JSON_UNLIKELY(first.m_object != last.m_object)) + { + JSON_THROW(invalid_iterator::create(210, "iterators do not fit")); + } + + if (JSON_UNLIKELY(first.m_object == this)) + { + JSON_THROW(invalid_iterator::create(211, "passed iterators may not belong to container")); + } + + // insert to array and return iterator + return insert_iterator(pos, first.m_it.array_iterator, last.m_it.array_iterator); + } + + /*! + @brief inserts elements + + Inserts elements from initializer list @a ilist before iterator @a pos. + + @param[in] pos iterator before which the content will be inserted; may be + the end() iterator + @param[in] ilist initializer list to insert the values from + + @throw type_error.309 if called on JSON values other than arrays; example: + `"cannot use insert() with string"` + @throw invalid_iterator.202 if @a pos is not an iterator of *this; + example: `"iterator does not fit current value"` + + @return iterator pointing to the first element inserted, or @a pos if + `ilist` is empty + + @complexity Linear in `ilist.size()` plus linear in the distance between + @a pos and end of the container. + + @liveexample{The example shows how `insert()` is used.,insert__ilist} + + @since version 1.0.0 + */ + iterator insert(const_iterator pos, initializer_list_t ilist) + { + // insert only works for arrays + if (JSON_UNLIKELY(not is_array())) + { + JSON_THROW(type_error::create(309, "cannot use insert() with " + std::string(type_name()))); + } + + // check if iterator pos fits to this JSON value + if (JSON_UNLIKELY(pos.m_object != this)) + { + JSON_THROW(invalid_iterator::create(202, "iterator does not fit current value")); + } + + // insert to array and return iterator + return insert_iterator(pos, ilist.begin(), ilist.end()); + } + + /*! + @brief inserts elements + + Inserts elements from range `[first, last)`. + + @param[in] first begin of the range of elements to insert + @param[in] last end of the range of elements to insert + + @throw type_error.309 if called on JSON values other than objects; example: + `"cannot use insert() with string"` + @throw invalid_iterator.202 if iterator @a first or @a last does does not + point to an object; example: `"iterators first and last must point to + objects"` + @throw invalid_iterator.210 if @a first and @a last do not belong to the + same JSON value; example: `"iterators do not fit"` + + @complexity Logarithmic: `O(N*log(size() + N))`, where `N` is the number + of elements to insert. + + @liveexample{The example shows how `insert()` is used.,insert__range_object} + + @since version 3.0.0 + */ + void insert(const_iterator first, const_iterator last) + { + // insert only works for objects + if (JSON_UNLIKELY(not is_object())) + { + JSON_THROW(type_error::create(309, "cannot use insert() with " + std::string(type_name()))); + } + + // check if range iterators belong to the same JSON object + if (JSON_UNLIKELY(first.m_object != last.m_object)) + { + JSON_THROW(invalid_iterator::create(210, "iterators do not fit")); + } + + // passed iterators must belong to objects + if (JSON_UNLIKELY(not first.m_object->is_object())) + { + JSON_THROW(invalid_iterator::create(202, "iterators first and last must point to objects")); + } + + m_value.object->insert(first.m_it.object_iterator, last.m_it.object_iterator); + } + + /*! + @brief updates a JSON object from another object, overwriting existing keys + + Inserts all values from JSON object @a j and overwrites existing keys. + + @param[in] j JSON object to read values from + + @throw type_error.312 if called on JSON values other than objects; example: + `"cannot use update() with string"` + + @complexity O(N*log(size() + N)), where N is the number of elements to + insert. + + @liveexample{The example shows how `update()` is used.,update} + + @sa https://docs.python.org/3.6/library/stdtypes.html#dict.update + + @since version 3.0.0 + */ + void update(const_reference j) + { + // implicitly convert null value to an empty object + if (is_null()) + { + m_type = value_t::object; + m_value.object = create<object_t>(); + assert_invariant(); + } + + if (JSON_UNLIKELY(not is_object())) + { + JSON_THROW(type_error::create(312, "cannot use update() with " + std::string(type_name()))); + } + if (JSON_UNLIKELY(not j.is_object())) + { + JSON_THROW(type_error::create(312, "cannot use update() with " + std::string(j.type_name()))); + } + + for (auto it = j.cbegin(); it != j.cend(); ++it) + { + m_value.object->operator[](it.key()) = it.value(); + } + } + + /*! + @brief updates a JSON object from another object, overwriting existing keys + + Inserts all values from from range `[first, last)` and overwrites existing + keys. + + @param[in] first begin of the range of elements to insert + @param[in] last end of the range of elements to insert + + @throw type_error.312 if called on JSON values other than objects; example: + `"cannot use update() with string"` + @throw invalid_iterator.202 if iterator @a first or @a last does does not + point to an object; example: `"iterators first and last must point to + objects"` + @throw invalid_iterator.210 if @a first and @a last do not belong to the + same JSON value; example: `"iterators do not fit"` + + @complexity O(N*log(size() + N)), where N is the number of elements to + insert. + + @liveexample{The example shows how `update()` is used__range.,update} + + @sa https://docs.python.org/3.6/library/stdtypes.html#dict.update + + @since version 3.0.0 + */ + void update(const_iterator first, const_iterator last) + { + // implicitly convert null value to an empty object + if (is_null()) + { + m_type = value_t::object; + m_value.object = create<object_t>(); + assert_invariant(); + } + + if (JSON_UNLIKELY(not is_object())) + { + JSON_THROW(type_error::create(312, "cannot use update() with " + std::string(type_name()))); + } + + // check if range iterators belong to the same JSON object + if (JSON_UNLIKELY(first.m_object != last.m_object)) + { + JSON_THROW(invalid_iterator::create(210, "iterators do not fit")); + } + + // passed iterators must belong to objects + if (JSON_UNLIKELY(not first.m_object->is_object() + or not last.m_object->is_object())) + { + JSON_THROW(invalid_iterator::create(202, "iterators first and last must point to objects")); + } + + for (auto it = first; it != last; ++it) + { + m_value.object->operator[](it.key()) = it.value(); + } + } + + /*! + @brief exchanges the values + + Exchanges the contents of the JSON value with those of @a other. Does not + invoke any move, copy, or swap operations on individual elements. All + iterators and references remain valid. The past-the-end iterator is + invalidated. + + @param[in,out] other JSON value to exchange the contents with + + @complexity Constant. + + @liveexample{The example below shows how JSON values can be swapped with + `swap()`.,swap__reference} + + @since version 1.0.0 + */ + void swap(reference other) noexcept ( + std::is_nothrow_move_constructible<value_t>::value and + std::is_nothrow_move_assignable<value_t>::value and + std::is_nothrow_move_constructible<json_value>::value and + std::is_nothrow_move_assignable<json_value>::value + ) + { + std::swap(m_type, other.m_type); + std::swap(m_value, other.m_value); + assert_invariant(); + } + + /*! + @brief exchanges the values + + Exchanges the contents of a JSON array with those of @a other. Does not + invoke any move, copy, or swap operations on individual elements. All + iterators and references remain valid. The past-the-end iterator is + invalidated. + + @param[in,out] other array to exchange the contents with + + @throw type_error.310 when JSON value is not an array; example: `"cannot + use swap() with string"` + + @complexity Constant. + + @liveexample{The example below shows how arrays can be swapped with + `swap()`.,swap__array_t} + + @since version 1.0.0 + */ + void swap(array_t& other) + { + // swap only works for arrays + if (JSON_LIKELY(is_array())) + { + std::swap(*(m_value.array), other); + } + else + { + JSON_THROW(type_error::create(310, "cannot use swap() with " + std::string(type_name()))); + } + } + + /*! + @brief exchanges the values + + Exchanges the contents of a JSON object with those of @a other. Does not + invoke any move, copy, or swap operations on individual elements. All + iterators and references remain valid. The past-the-end iterator is + invalidated. + + @param[in,out] other object to exchange the contents with + + @throw type_error.310 when JSON value is not an object; example: + `"cannot use swap() with string"` + + @complexity Constant. + + @liveexample{The example below shows how objects can be swapped with + `swap()`.,swap__object_t} + + @since version 1.0.0 + */ + void swap(object_t& other) + { + // swap only works for objects + if (JSON_LIKELY(is_object())) + { + std::swap(*(m_value.object), other); + } + else + { + JSON_THROW(type_error::create(310, "cannot use swap() with " + std::string(type_name()))); + } + } + + /*! + @brief exchanges the values + + Exchanges the contents of a JSON string with those of @a other. Does not + invoke any move, copy, or swap operations on individual elements. All + iterators and references remain valid. The past-the-end iterator is + invalidated. + + @param[in,out] other string to exchange the contents with + + @throw type_error.310 when JSON value is not a string; example: `"cannot + use swap() with boolean"` + + @complexity Constant. + + @liveexample{The example below shows how strings can be swapped with + `swap()`.,swap__string_t} + + @since version 1.0.0 + */ + void swap(string_t& other) + { + // swap only works for strings + if (JSON_LIKELY(is_string())) + { + std::swap(*(m_value.string), other); + } + else + { + JSON_THROW(type_error::create(310, "cannot use swap() with " + std::string(type_name()))); + } + } + + /// @} + + public: + ////////////////////////////////////////// + // lexicographical comparison operators // + ////////////////////////////////////////// + + /// @name lexicographical comparison operators + /// @{ + + /*! + @brief comparison: equal + + Compares two JSON values for equality according to the following rules: + - Two JSON values are equal if (1) they are from the same type and (2) + their stored values are the same according to their respective + `operator==`. + - Integer and floating-point numbers are automatically converted before + comparison. Note than two NaN values are always treated as unequal. + - Two JSON null values are equal. + + @note Floating-point inside JSON values numbers are compared with + `json::number_float_t::operator==` which is `double::operator==` by + default. To compare floating-point while respecting an epsilon, an alternative + [comparison function](https://github.com/mariokonrad/marnav/blob/master/src/marnav/math/floatingpoint.hpp#L34-#L39) + could be used, for instance + @code {.cpp} + template<typename T, typename = typename std::enable_if<std::is_floating_point<T>::value, T>::type> + inline bool is_same(T a, T b, T epsilon = std::numeric_limits<T>::epsilon()) noexcept + { + return std::abs(a - b) <= epsilon; + } + @endcode + + @note NaN values never compare equal to themselves or to other NaN values. + + @param[in] lhs first JSON value to consider + @param[in] rhs second JSON value to consider + @return whether the values @a lhs and @a rhs are equal + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @complexity Linear. + + @liveexample{The example demonstrates comparing several JSON + types.,operator__equal} + + @since version 1.0.0 + */ + friend bool operator==(const_reference lhs, const_reference rhs) noexcept + { + const auto lhs_type = lhs.type(); + const auto rhs_type = rhs.type(); + + if (lhs_type == rhs_type) + { + switch (lhs_type) + { + case value_t::array: + return (*lhs.m_value.array == *rhs.m_value.array); + + case value_t::object: + return (*lhs.m_value.object == *rhs.m_value.object); + + case value_t::null: + return true; + + case value_t::string: + return (*lhs.m_value.string == *rhs.m_value.string); + + case value_t::boolean: + return (lhs.m_value.boolean == rhs.m_value.boolean); + + case value_t::number_integer: + return (lhs.m_value.number_integer == rhs.m_value.number_integer); + + case value_t::number_unsigned: + return (lhs.m_value.number_unsigned == rhs.m_value.number_unsigned); + + case value_t::number_float: + return (lhs.m_value.number_float == rhs.m_value.number_float); + + default: + return false; + } + } + else if (lhs_type == value_t::number_integer and rhs_type == value_t::number_float) + { + return (static_cast<number_float_t>(lhs.m_value.number_integer) == rhs.m_value.number_float); + } + else if (lhs_type == value_t::number_float and rhs_type == value_t::number_integer) + { + return (lhs.m_value.number_float == static_cast<number_float_t>(rhs.m_value.number_integer)); + } + else if (lhs_type == value_t::number_unsigned and rhs_type == value_t::number_float) + { + return (static_cast<number_float_t>(lhs.m_value.number_unsigned) == rhs.m_value.number_float); + } + else if (lhs_type == value_t::number_float and rhs_type == value_t::number_unsigned) + { + return (lhs.m_value.number_float == static_cast<number_float_t>(rhs.m_value.number_unsigned)); + } + else if (lhs_type == value_t::number_unsigned and rhs_type == value_t::number_integer) + { + return (static_cast<number_integer_t>(lhs.m_value.number_unsigned) == rhs.m_value.number_integer); + } + else if (lhs_type == value_t::number_integer and rhs_type == value_t::number_unsigned) + { + return (lhs.m_value.number_integer == static_cast<number_integer_t>(rhs.m_value.number_unsigned)); + } + + return false; + } + + /*! + @brief comparison: equal + @copydoc operator==(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator==(const_reference lhs, const ScalarType rhs) noexcept + { + return (lhs == basic_json(rhs)); + } + + /*! + @brief comparison: equal + @copydoc operator==(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator==(const ScalarType lhs, const_reference rhs) noexcept + { + return (basic_json(lhs) == rhs); + } + + /*! + @brief comparison: not equal + + Compares two JSON values for inequality by calculating `not (lhs == rhs)`. + + @param[in] lhs first JSON value to consider + @param[in] rhs second JSON value to consider + @return whether the values @a lhs and @a rhs are not equal + + @complexity Linear. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @liveexample{The example demonstrates comparing several JSON + types.,operator__notequal} + + @since version 1.0.0 + */ + friend bool operator!=(const_reference lhs, const_reference rhs) noexcept + { + return not (lhs == rhs); + } + + /*! + @brief comparison: not equal + @copydoc operator!=(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator!=(const_reference lhs, const ScalarType rhs) noexcept + { + return (lhs != basic_json(rhs)); + } + + /*! + @brief comparison: not equal + @copydoc operator!=(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator!=(const ScalarType lhs, const_reference rhs) noexcept + { + return (basic_json(lhs) != rhs); + } + + /*! + @brief comparison: less than + + Compares whether one JSON value @a lhs is less than another JSON value @a + rhs according to the following rules: + - If @a lhs and @a rhs have the same type, the values are compared using + the default `<` operator. + - Integer and floating-point numbers are automatically converted before + comparison + - In case @a lhs and @a rhs have different types, the values are ignored + and the order of the types is considered, see + @ref operator<(const value_t, const value_t). + + @param[in] lhs first JSON value to consider + @param[in] rhs second JSON value to consider + @return whether @a lhs is less than @a rhs + + @complexity Linear. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @liveexample{The example demonstrates comparing several JSON + types.,operator__less} + + @since version 1.0.0 + */ + friend bool operator<(const_reference lhs, const_reference rhs) noexcept + { + const auto lhs_type = lhs.type(); + const auto rhs_type = rhs.type(); + + if (lhs_type == rhs_type) + { + switch (lhs_type) + { + case value_t::array: + return (*lhs.m_value.array) < (*rhs.m_value.array); + + case value_t::object: + return *lhs.m_value.object < *rhs.m_value.object; + + case value_t::null: + return false; + + case value_t::string: + return *lhs.m_value.string < *rhs.m_value.string; + + case value_t::boolean: + return lhs.m_value.boolean < rhs.m_value.boolean; + + case value_t::number_integer: + return lhs.m_value.number_integer < rhs.m_value.number_integer; + + case value_t::number_unsigned: + return lhs.m_value.number_unsigned < rhs.m_value.number_unsigned; + + case value_t::number_float: + return lhs.m_value.number_float < rhs.m_value.number_float; + + default: + return false; + } + } + else if (lhs_type == value_t::number_integer and rhs_type == value_t::number_float) + { + return static_cast<number_float_t>(lhs.m_value.number_integer) < rhs.m_value.number_float; + } + else if (lhs_type == value_t::number_float and rhs_type == value_t::number_integer) + { + return lhs.m_value.number_float < static_cast<number_float_t>(rhs.m_value.number_integer); + } + else if (lhs_type == value_t::number_unsigned and rhs_type == value_t::number_float) + { + return static_cast<number_float_t>(lhs.m_value.number_unsigned) < rhs.m_value.number_float; + } + else if (lhs_type == value_t::number_float and rhs_type == value_t::number_unsigned) + { + return lhs.m_value.number_float < static_cast<number_float_t>(rhs.m_value.number_unsigned); + } + else if (lhs_type == value_t::number_integer and rhs_type == value_t::number_unsigned) + { + return lhs.m_value.number_integer < static_cast<number_integer_t>(rhs.m_value.number_unsigned); + } + else if (lhs_type == value_t::number_unsigned and rhs_type == value_t::number_integer) + { + return static_cast<number_integer_t>(lhs.m_value.number_unsigned) < rhs.m_value.number_integer; + } + + // We only reach this line if we cannot compare values. In that case, + // we compare types. Note we have to call the operator explicitly, + // because MSVC has problems otherwise. + return operator<(lhs_type, rhs_type); + } + + /*! + @brief comparison: less than + @copydoc operator<(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator<(const_reference lhs, const ScalarType rhs) noexcept + { + return (lhs < basic_json(rhs)); + } + + /*! + @brief comparison: less than + @copydoc operator<(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator<(const ScalarType lhs, const_reference rhs) noexcept + { + return (basic_json(lhs) < rhs); + } + + /*! + @brief comparison: less than or equal + + Compares whether one JSON value @a lhs is less than or equal to another + JSON value by calculating `not (rhs < lhs)`. + + @param[in] lhs first JSON value to consider + @param[in] rhs second JSON value to consider + @return whether @a lhs is less than or equal to @a rhs + + @complexity Linear. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @liveexample{The example demonstrates comparing several JSON + types.,operator__greater} + + @since version 1.0.0 + */ + friend bool operator<=(const_reference lhs, const_reference rhs) noexcept + { + return not (rhs < lhs); + } + + /*! + @brief comparison: less than or equal + @copydoc operator<=(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator<=(const_reference lhs, const ScalarType rhs) noexcept + { + return (lhs <= basic_json(rhs)); + } + + /*! + @brief comparison: less than or equal + @copydoc operator<=(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator<=(const ScalarType lhs, const_reference rhs) noexcept + { + return (basic_json(lhs) <= rhs); + } + + /*! + @brief comparison: greater than + + Compares whether one JSON value @a lhs is greater than another + JSON value by calculating `not (lhs <= rhs)`. + + @param[in] lhs first JSON value to consider + @param[in] rhs second JSON value to consider + @return whether @a lhs is greater than to @a rhs + + @complexity Linear. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @liveexample{The example demonstrates comparing several JSON + types.,operator__lessequal} + + @since version 1.0.0 + */ + friend bool operator>(const_reference lhs, const_reference rhs) noexcept + { + return not (lhs <= rhs); + } + + /*! + @brief comparison: greater than + @copydoc operator>(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator>(const_reference lhs, const ScalarType rhs) noexcept + { + return (lhs > basic_json(rhs)); + } + + /*! + @brief comparison: greater than + @copydoc operator>(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator>(const ScalarType lhs, const_reference rhs) noexcept + { + return (basic_json(lhs) > rhs); + } + + /*! + @brief comparison: greater than or equal + + Compares whether one JSON value @a lhs is greater than or equal to another + JSON value by calculating `not (lhs < rhs)`. + + @param[in] lhs first JSON value to consider + @param[in] rhs second JSON value to consider + @return whether @a lhs is greater than or equal to @a rhs + + @complexity Linear. + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @liveexample{The example demonstrates comparing several JSON + types.,operator__greaterequal} + + @since version 1.0.0 + */ + friend bool operator>=(const_reference lhs, const_reference rhs) noexcept + { + return not (lhs < rhs); + } + + /*! + @brief comparison: greater than or equal + @copydoc operator>=(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator>=(const_reference lhs, const ScalarType rhs) noexcept + { + return (lhs >= basic_json(rhs)); + } + + /*! + @brief comparison: greater than or equal + @copydoc operator>=(const_reference, const_reference) + */ + template<typename ScalarType, typename std::enable_if< + std::is_scalar<ScalarType>::value, int>::type = 0> + friend bool operator>=(const ScalarType lhs, const_reference rhs) noexcept + { + return (basic_json(lhs) >= rhs); + } + + /// @} + + /////////////////// + // serialization // + /////////////////// + + /// @name serialization + /// @{ + + /*! + @brief serialize to stream + + Serialize the given JSON value @a j to the output stream @a o. The JSON + value will be serialized using the @ref dump member function. + + - The indentation of the output can be controlled with the member variable + `width` of the output stream @a o. For instance, using the manipulator + `std::setw(4)` on @a o sets the indentation level to `4` and the + serialization result is the same as calling `dump(4)`. + + - The indentation character can be controlled with the member variable + `fill` of the output stream @a o. For instance, the manipulator + `std::setfill('\\t')` sets indentation to use a tab character rather than + the default space character. + + @param[in,out] o stream to serialize to + @param[in] j JSON value to serialize + + @return the stream @a o + + @throw type_error.316 if a string stored inside the JSON value is not + UTF-8 encoded + + @complexity Linear. + + @liveexample{The example below shows the serialization with different + parameters to `width` to adjust the indentation level.,operator_serialize} + + @since version 1.0.0; indentation character added in version 3.0.0 + */ + friend std::ostream& operator<<(std::ostream& o, const basic_json& j) + { + // read width member and use it as indentation parameter if nonzero + const bool pretty_print = (o.width() > 0); + const auto indentation = (pretty_print ? o.width() : 0); + + // reset width to 0 for subsequent calls to this stream + o.width(0); + + // do the actual serialization + serializer s(detail::output_adapter<char>(o), o.fill()); + s.dump(j, pretty_print, false, static_cast<unsigned int>(indentation)); + return o; + } + + /*! + @brief serialize to stream + @deprecated This stream operator is deprecated and will be removed in + future 4.0.0 of the library. Please use + @ref operator<<(std::ostream&, const basic_json&) + instead; that is, replace calls like `j >> o;` with `o << j;`. + @since version 1.0.0; deprecated since version 3.0.0 + */ + JSON_DEPRECATED + friend std::ostream& operator>>(const basic_json& j, std::ostream& o) + { + return o << j; + } + + /// @} + + + ///////////////////// + // deserialization // + ///////////////////// + + /// @name deserialization + /// @{ + + /*! + @brief deserialize from a compatible input + + This function reads from a compatible input. Examples are: + - an array of 1-byte values + - strings with character/literal type with size of 1 byte + - input streams + - container with contiguous storage of 1-byte values. Compatible container + types include `std::vector`, `std::string`, `std::array`, + `std::valarray`, and `std::initializer_list`. Furthermore, C-style + arrays can be used with `std::begin()`/`std::end()`. User-defined + containers can be used as long as they implement random-access iterators + and a contiguous storage. + + @pre Each element of the container has a size of 1 byte. Violating this + precondition yields undefined behavior. **This precondition is enforced + with a static assertion.** + + @pre The container storage is contiguous. Violating this precondition + yields undefined behavior. **This precondition is enforced with an + assertion.** + @pre Each element of the container has a size of 1 byte. Violating this + precondition yields undefined behavior. **This precondition is enforced + with a static assertion.** + + @warning There is no way to enforce all preconditions at compile-time. If + the function is called with a noncompliant container and with + assertions switched off, the behavior is undefined and will most + likely yield segmentation violation. + + @param[in] i input to read from + @param[in] cb a parser callback function of type @ref parser_callback_t + which is used to control the deserialization by filtering unwanted values + (optional) + @param[in] allow_exceptions whether to throw exceptions in case of a + parse error (optional, true by default) + + @return result of the deserialization + + @throw parse_error.101 if a parse error occurs; example: `""unexpected end + of input; expected string literal""` + @throw parse_error.102 if to_unicode fails or surrogate error + @throw parse_error.103 if to_unicode fails + + @complexity Linear in the length of the input. The parser is a predictive + LL(1) parser. The complexity can be higher if the parser callback function + @a cb has a super-linear complexity. + + @note A UTF-8 byte order mark is silently ignored. + + @liveexample{The example below demonstrates the `parse()` function reading + from an array.,parse__array__parser_callback_t} + + @liveexample{The example below demonstrates the `parse()` function with + and without callback function.,parse__string__parser_callback_t} + + @liveexample{The example below demonstrates the `parse()` function with + and without callback function.,parse__istream__parser_callback_t} + + @liveexample{The example below demonstrates the `parse()` function reading + from a contiguous container.,parse__contiguouscontainer__parser_callback_t} + + @since version 2.0.3 (contiguous containers) + */ + static basic_json parse(detail::input_adapter&& i, + const parser_callback_t cb = nullptr, + const bool allow_exceptions = true) + { + basic_json result; + parser(i, cb, allow_exceptions).parse(true, result); + return result; + } + + static bool accept(detail::input_adapter&& i) + { + return parser(i).accept(true); + } + + /*! + @brief generate SAX events + + The SAX event lister must follow the interface of @ref json_sax. + + This function reads from a compatible input. Examples are: + - an array of 1-byte values + - strings with character/literal type with size of 1 byte + - input streams + - container with contiguous storage of 1-byte values. Compatible container + types include `std::vector`, `std::string`, `std::array`, + `std::valarray`, and `std::initializer_list`. Furthermore, C-style + arrays can be used with `std::begin()`/`std::end()`. User-defined + containers can be used as long as they implement random-access iterators + and a contiguous storage. + + @pre Each element of the container has a size of 1 byte. Violating this + precondition yields undefined behavior. **This precondition is enforced + with a static assertion.** + + @pre The container storage is contiguous. Violating this precondition + yields undefined behavior. **This precondition is enforced with an + assertion.** + @pre Each element of the container has a size of 1 byte. Violating this + precondition yields undefined behavior. **This precondition is enforced + with a static assertion.** + + @warning There is no way to enforce all preconditions at compile-time. If + the function is called with a noncompliant container and with + assertions switched off, the behavior is undefined and will most + likely yield segmentation violation. + + @param[in] i input to read from + @param[in,out] sax SAX event listener + @param[in] format the format to parse (JSON, CBOR, MessagePack, or UBJSON) + @param[in] strict whether the input has to be consumed completely + + @return return value of the last processed SAX event + + @throw parse_error.101 if a parse error occurs; example: `""unexpected end + of input; expected string literal""` + @throw parse_error.102 if to_unicode fails or surrogate error + @throw parse_error.103 if to_unicode fails + + @complexity Linear in the length of the input. The parser is a predictive + LL(1) parser. The complexity can be higher if the SAX consumer @a sax has + a super-linear complexity. + + @note A UTF-8 byte order mark is silently ignored. + + @liveexample{The example below demonstrates the `sax_parse()` function + reading from string and processing the events with a user-defined SAX + event consumer.,sax_parse} + + @since version 3.2.0 + */ + template <typename SAX> + static bool sax_parse(detail::input_adapter&& i, SAX* sax, + input_format_t format = input_format_t::json, + const bool strict = true) + { + assert(sax); + switch (format) + { + case input_format_t::json: + return parser(std::move(i)).sax_parse(sax, strict); + default: + return detail::binary_reader<basic_json, SAX>(std::move(i)).sax_parse(format, sax, strict); + } + } + + /*! + @brief deserialize from an iterator range with contiguous storage + + This function reads from an iterator range of a container with contiguous + storage of 1-byte values. Compatible container types include + `std::vector`, `std::string`, `std::array`, `std::valarray`, and + `std::initializer_list`. Furthermore, C-style arrays can be used with + `std::begin()`/`std::end()`. User-defined containers can be used as long + as they implement random-access iterators and a contiguous storage. + + @pre The iterator range is contiguous. Violating this precondition yields + undefined behavior. **This precondition is enforced with an assertion.** + @pre Each element in the range has a size of 1 byte. Violating this + precondition yields undefined behavior. **This precondition is enforced + with a static assertion.** + + @warning There is no way to enforce all preconditions at compile-time. If + the function is called with noncompliant iterators and with + assertions switched off, the behavior is undefined and will most + likely yield segmentation violation. + + @tparam IteratorType iterator of container with contiguous storage + @param[in] first begin of the range to parse (included) + @param[in] last end of the range to parse (excluded) + @param[in] cb a parser callback function of type @ref parser_callback_t + which is used to control the deserialization by filtering unwanted values + (optional) + @param[in] allow_exceptions whether to throw exceptions in case of a + parse error (optional, true by default) + + @return result of the deserialization + + @throw parse_error.101 in case of an unexpected token + @throw parse_error.102 if to_unicode fails or surrogate error + @throw parse_error.103 if to_unicode fails + + @complexity Linear in the length of the input. The parser is a predictive + LL(1) parser. The complexity can be higher if the parser callback function + @a cb has a super-linear complexity. + + @note A UTF-8 byte order mark is silently ignored. + + @liveexample{The example below demonstrates the `parse()` function reading + from an iterator range.,parse__iteratortype__parser_callback_t} + + @since version 2.0.3 + */ + template<class IteratorType, typename std::enable_if< + std::is_base_of< + std::random_access_iterator_tag, + typename std::iterator_traits<IteratorType>::iterator_category>::value, int>::type = 0> + static basic_json parse(IteratorType first, IteratorType last, + const parser_callback_t cb = nullptr, + const bool allow_exceptions = true) + { + basic_json result; + parser(detail::input_adapter(first, last), cb, allow_exceptions).parse(true, result); + return result; + } + + template<class IteratorType, typename std::enable_if< + std::is_base_of< + std::random_access_iterator_tag, + typename std::iterator_traits<IteratorType>::iterator_category>::value, int>::type = 0> + static bool accept(IteratorType first, IteratorType last) + { + return parser(detail::input_adapter(first, last)).accept(true); + } + + template<class IteratorType, class SAX, typename std::enable_if< + std::is_base_of< + std::random_access_iterator_tag, + typename std::iterator_traits<IteratorType>::iterator_category>::value, int>::type = 0> + static bool sax_parse(IteratorType first, IteratorType last, SAX* sax) + { + return parser(detail::input_adapter(first, last)).sax_parse(sax); + } + + /*! + @brief deserialize from stream + @deprecated This stream operator is deprecated and will be removed in + version 4.0.0 of the library. Please use + @ref operator>>(std::istream&, basic_json&) + instead; that is, replace calls like `j << i;` with `i >> j;`. + @since version 1.0.0; deprecated since version 3.0.0 + */ + JSON_DEPRECATED + friend std::istream& operator<<(basic_json& j, std::istream& i) + { + return operator>>(i, j); + } + + /*! + @brief deserialize from stream + + Deserializes an input stream to a JSON value. + + @param[in,out] i input stream to read a serialized JSON value from + @param[in,out] j JSON value to write the deserialized input to + + @throw parse_error.101 in case of an unexpected token + @throw parse_error.102 if to_unicode fails or surrogate error + @throw parse_error.103 if to_unicode fails + + @complexity Linear in the length of the input. The parser is a predictive + LL(1) parser. + + @note A UTF-8 byte order mark is silently ignored. + + @liveexample{The example below shows how a JSON value is constructed by + reading a serialization from a stream.,operator_deserialize} + + @sa parse(std::istream&, const parser_callback_t) for a variant with a + parser callback function to filter values while parsing + + @since version 1.0.0 + */ + friend std::istream& operator>>(std::istream& i, basic_json& j) + { + parser(detail::input_adapter(i)).parse(false, j); + return i; + } + + /// @} + + /////////////////////////// + // convenience functions // + /////////////////////////// + + /*! + @brief return the type as string + + Returns the type name as string to be used in error messages - usually to + indicate that a function was called on a wrong JSON type. + + @return a string representation of a the @a m_type member: + Value type | return value + ----------- | ------------- + null | `"null"` + boolean | `"boolean"` + string | `"string"` + number | `"number"` (for all number types) + object | `"object"` + array | `"array"` + discarded | `"discarded"` + + @exceptionsafety No-throw guarantee: this function never throws exceptions. + + @complexity Constant. + + @liveexample{The following code exemplifies `type_name()` for all JSON + types.,type_name} + + @sa @ref type() -- return the type of the JSON value + @sa @ref operator value_t() -- return the type of the JSON value (implicit) + + @since version 1.0.0, public since 2.1.0, `const char*` and `noexcept` + since 3.0.0 + */ + const char* type_name() const noexcept + { + { + switch (m_type) + { + case value_t::null: + return "null"; + case value_t::object: + return "object"; + case value_t::array: + return "array"; + case value_t::string: + return "string"; + case value_t::boolean: + return "boolean"; + case value_t::discarded: + return "discarded"; + default: + return "number"; + } + } + } + + + private: + ////////////////////// + // member variables // + ////////////////////// + + /// the type of the current element + value_t m_type = value_t::null; + + /// the value of the current element + json_value m_value = {}; + + ////////////////////////////////////////// + // binary serialization/deserialization // + ////////////////////////////////////////// + + /// @name binary serialization/deserialization support + /// @{ + + public: + /*! + @brief create a CBOR serialization of a given JSON value + + Serializes a given JSON value @a j to a byte vector using the CBOR (Concise + Binary Object Representation) serialization format. CBOR is a binary + serialization format which aims to be more compact than JSON itself, yet + more efficient to parse. + + The library uses the following mapping from JSON values types to + CBOR types according to the CBOR specification (RFC 7049): + + JSON value type | value/range | CBOR type | first byte + --------------- | ------------------------------------------ | ---------------------------------- | --------------- + null | `null` | Null | 0xF6 + boolean | `true` | True | 0xF5 + boolean | `false` | False | 0xF4 + number_integer | -9223372036854775808..-2147483649 | Negative integer (8 bytes follow) | 0x3B + number_integer | -2147483648..-32769 | Negative integer (4 bytes follow) | 0x3A + number_integer | -32768..-129 | Negative integer (2 bytes follow) | 0x39 + number_integer | -128..-25 | Negative integer (1 byte follow) | 0x38 + number_integer | -24..-1 | Negative integer | 0x20..0x37 + number_integer | 0..23 | Integer | 0x00..0x17 + number_integer | 24..255 | Unsigned integer (1 byte follow) | 0x18 + number_integer | 256..65535 | Unsigned integer (2 bytes follow) | 0x19 + number_integer | 65536..4294967295 | Unsigned integer (4 bytes follow) | 0x1A + number_integer | 4294967296..18446744073709551615 | Unsigned integer (8 bytes follow) | 0x1B + number_unsigned | 0..23 | Integer | 0x00..0x17 + number_unsigned | 24..255 | Unsigned integer (1 byte follow) | 0x18 + number_unsigned | 256..65535 | Unsigned integer (2 bytes follow) | 0x19 + number_unsigned | 65536..4294967295 | Unsigned integer (4 bytes follow) | 0x1A + number_unsigned | 4294967296..18446744073709551615 | Unsigned integer (8 bytes follow) | 0x1B + number_float | *any value* | Double-Precision Float | 0xFB + string | *length*: 0..23 | UTF-8 string | 0x60..0x77 + string | *length*: 23..255 | UTF-8 string (1 byte follow) | 0x78 + string | *length*: 256..65535 | UTF-8 string (2 bytes follow) | 0x79 + string | *length*: 65536..4294967295 | UTF-8 string (4 bytes follow) | 0x7A + string | *length*: 4294967296..18446744073709551615 | UTF-8 string (8 bytes follow) | 0x7B + array | *size*: 0..23 | array | 0x80..0x97 + array | *size*: 23..255 | array (1 byte follow) | 0x98 + array | *size*: 256..65535 | array (2 bytes follow) | 0x99 + array | *size*: 65536..4294967295 | array (4 bytes follow) | 0x9A + array | *size*: 4294967296..18446744073709551615 | array (8 bytes follow) | 0x9B + object | *size*: 0..23 | map | 0xA0..0xB7 + object | *size*: 23..255 | map (1 byte follow) | 0xB8 + object | *size*: 256..65535 | map (2 bytes follow) | 0xB9 + object | *size*: 65536..4294967295 | map (4 bytes follow) | 0xBA + object | *size*: 4294967296..18446744073709551615 | map (8 bytes follow) | 0xBB + + @note The mapping is **complete** in the sense that any JSON value type + can be converted to a CBOR value. + + @note If NaN or Infinity are stored inside a JSON number, they are + serialized properly. This behavior differs from the @ref dump() + function which serializes NaN or Infinity to `null`. + + @note The following CBOR types are not used in the conversion: + - byte strings (0x40..0x5F) + - UTF-8 strings terminated by "break" (0x7F) + - arrays terminated by "break" (0x9F) + - maps terminated by "break" (0xBF) + - date/time (0xC0..0xC1) + - bignum (0xC2..0xC3) + - decimal fraction (0xC4) + - bigfloat (0xC5) + - tagged items (0xC6..0xD4, 0xD8..0xDB) + - expected conversions (0xD5..0xD7) + - simple values (0xE0..0xF3, 0xF8) + - undefined (0xF7) + - half and single-precision floats (0xF9-0xFA) + - break (0xFF) + + @param[in] j JSON value to serialize + @return MessagePack serialization as byte vector + + @complexity Linear in the size of the JSON value @a j. + + @liveexample{The example shows the serialization of a JSON value to a byte + vector in CBOR format.,to_cbor} + + @sa http://cbor.io + @sa @ref from_cbor(detail::input_adapter&&, const bool, const bool) for the + analogous deserialization + @sa @ref to_msgpack(const basic_json&) for the related MessagePack format + @sa @ref to_ubjson(const basic_json&, const bool, const bool) for the + related UBJSON format + + @since version 2.0.9 + */ + static std::vector<uint8_t> to_cbor(const basic_json& j) + { + std::vector<uint8_t> result; + to_cbor(j, result); + return result; + } + + static void to_cbor(const basic_json& j, detail::output_adapter<uint8_t> o) + { + binary_writer<uint8_t>(o).write_cbor(j); + } + + static void to_cbor(const basic_json& j, detail::output_adapter<char> o) + { + binary_writer<char>(o).write_cbor(j); + } + + /*! + @brief create a MessagePack serialization of a given JSON value + + Serializes a given JSON value @a j to a byte vector using the MessagePack + serialization format. MessagePack is a binary serialization format which + aims to be more compact than JSON itself, yet more efficient to parse. + + The library uses the following mapping from JSON values types to + MessagePack types according to the MessagePack specification: + + JSON value type | value/range | MessagePack type | first byte + --------------- | --------------------------------- | ---------------- | ---------- + null | `null` | nil | 0xC0 + boolean | `true` | true | 0xC3 + boolean | `false` | false | 0xC2 + number_integer | -9223372036854775808..-2147483649 | int64 | 0xD3 + number_integer | -2147483648..-32769 | int32 | 0xD2 + number_integer | -32768..-129 | int16 | 0xD1 + number_integer | -128..-33 | int8 | 0xD0 + number_integer | -32..-1 | negative fixint | 0xE0..0xFF + number_integer | 0..127 | positive fixint | 0x00..0x7F + number_integer | 128..255 | uint 8 | 0xCC + number_integer | 256..65535 | uint 16 | 0xCD + number_integer | 65536..4294967295 | uint 32 | 0xCE + number_integer | 4294967296..18446744073709551615 | uint 64 | 0xCF + number_unsigned | 0..127 | positive fixint | 0x00..0x7F + number_unsigned | 128..255 | uint 8 | 0xCC + number_unsigned | 256..65535 | uint 16 | 0xCD + number_unsigned | 65536..4294967295 | uint 32 | 0xCE + number_unsigned | 4294967296..18446744073709551615 | uint 64 | 0xCF + number_float | *any value* | float 64 | 0xCB + string | *length*: 0..31 | fixstr | 0xA0..0xBF + string | *length*: 32..255 | str 8 | 0xD9 + string | *length*: 256..65535 | str 16 | 0xDA + string | *length*: 65536..4294967295 | str 32 | 0xDB + array | *size*: 0..15 | fixarray | 0x90..0x9F + array | *size*: 16..65535 | array 16 | 0xDC + array | *size*: 65536..4294967295 | array 32 | 0xDD + object | *size*: 0..15 | fix map | 0x80..0x8F + object | *size*: 16..65535 | map 16 | 0xDE + object | *size*: 65536..4294967295 | map 32 | 0xDF + + @note The mapping is **complete** in the sense that any JSON value type + can be converted to a MessagePack value. + + @note The following values can **not** be converted to a MessagePack value: + - strings with more than 4294967295 bytes + - arrays with more than 4294967295 elements + - objects with more than 4294967295 elements + + @note The following MessagePack types are not used in the conversion: + - bin 8 - bin 32 (0xC4..0xC6) + - ext 8 - ext 32 (0xC7..0xC9) + - float 32 (0xCA) + - fixext 1 - fixext 16 (0xD4..0xD8) + + @note Any MessagePack output created @ref to_msgpack can be successfully + parsed by @ref from_msgpack. + + @note If NaN or Infinity are stored inside a JSON number, they are + serialized properly. This behavior differs from the @ref dump() + function which serializes NaN or Infinity to `null`. + + @param[in] j JSON value to serialize + @return MessagePack serialization as byte vector + + @complexity Linear in the size of the JSON value @a j. + + @liveexample{The example shows the serialization of a JSON value to a byte + vector in MessagePack format.,to_msgpack} + + @sa http://msgpack.org + @sa @ref from_msgpack for the analogous deserialization + @sa @ref to_cbor(const basic_json& for the related CBOR format + @sa @ref to_ubjson(const basic_json&, const bool, const bool) for the + related UBJSON format + + @since version 2.0.9 + */ + static std::vector<uint8_t> to_msgpack(const basic_json& j) + { + std::vector<uint8_t> result; + to_msgpack(j, result); + return result; + } + + static void to_msgpack(const basic_json& j, detail::output_adapter<uint8_t> o) + { + binary_writer<uint8_t>(o).write_msgpack(j); + } + + static void to_msgpack(const basic_json& j, detail::output_adapter<char> o) + { + binary_writer<char>(o).write_msgpack(j); + } + + /*! + @brief create a UBJSON serialization of a given JSON value + + Serializes a given JSON value @a j to a byte vector using the UBJSON + (Universal Binary JSON) serialization format. UBJSON aims to be more compact + than JSON itself, yet more efficient to parse. + + The library uses the following mapping from JSON values types to + UBJSON types according to the UBJSON specification: + + JSON value type | value/range | UBJSON type | marker + --------------- | --------------------------------- | ----------- | ------ + null | `null` | null | `Z` + boolean | `true` | true | `T` + boolean | `false` | false | `F` + number_integer | -9223372036854775808..-2147483649 | int64 | `L` + number_integer | -2147483648..-32769 | int32 | `l` + number_integer | -32768..-129 | int16 | `I` + number_integer | -128..127 | int8 | `i` + number_integer | 128..255 | uint8 | `U` + number_integer | 256..32767 | int16 | `I` + number_integer | 32768..2147483647 | int32 | `l` + number_integer | 2147483648..9223372036854775807 | int64 | `L` + number_unsigned | 0..127 | int8 | `i` + number_unsigned | 128..255 | uint8 | `U` + number_unsigned | 256..32767 | int16 | `I` + number_unsigned | 32768..2147483647 | int32 | `l` + number_unsigned | 2147483648..9223372036854775807 | int64 | `L` + number_float | *any value* | float64 | `D` + string | *with shortest length indicator* | string | `S` + array | *see notes on optimized format* | array | `[` + object | *see notes on optimized format* | map | `{` + + @note The mapping is **complete** in the sense that any JSON value type + can be converted to a UBJSON value. + + @note The following values can **not** be converted to a UBJSON value: + - strings with more than 9223372036854775807 bytes (theoretical) + - unsigned integer numbers above 9223372036854775807 + + @note The following markers are not used in the conversion: + - `Z`: no-op values are not created. + - `C`: single-byte strings are serialized with `S` markers. + + @note Any UBJSON output created @ref to_ubjson can be successfully parsed + by @ref from_ubjson. + + @note If NaN or Infinity are stored inside a JSON number, they are + serialized properly. This behavior differs from the @ref dump() + function which serializes NaN or Infinity to `null`. + + @note The optimized formats for containers are supported: Parameter + @a use_size adds size information to the beginning of a container and + removes the closing marker. Parameter @a use_type further checks + whether all elements of a container have the same type and adds the + type marker to the beginning of the container. The @a use_type + parameter must only be used together with @a use_size = true. Note + that @a use_size = true alone may result in larger representations - + the benefit of this parameter is that the receiving side is + immediately informed on the number of elements of the container. + + @param[in] j JSON value to serialize + @param[in] use_size whether to add size annotations to container types + @param[in] use_type whether to add type annotations to container types + (must be combined with @a use_size = true) + @return UBJSON serialization as byte vector + + @complexity Linear in the size of the JSON value @a j. + + @liveexample{The example shows the serialization of a JSON value to a byte + vector in UBJSON format.,to_ubjson} + + @sa http://ubjson.org + @sa @ref from_ubjson(detail::input_adapter&&, const bool, const bool) for the + analogous deserialization + @sa @ref to_cbor(const basic_json& for the related CBOR format + @sa @ref to_msgpack(const basic_json&) for the related MessagePack format + + @since version 3.1.0 + */ + static std::vector<uint8_t> to_ubjson(const basic_json& j, + const bool use_size = false, + const bool use_type = false) + { + std::vector<uint8_t> result; + to_ubjson(j, result, use_size, use_type); + return result; + } + + static void to_ubjson(const basic_json& j, detail::output_adapter<uint8_t> o, + const bool use_size = false, const bool use_type = false) + { + binary_writer<uint8_t>(o).write_ubjson(j, use_size, use_type); + } + + static void to_ubjson(const basic_json& j, detail::output_adapter<char> o, + const bool use_size = false, const bool use_type = false) + { + binary_writer<char>(o).write_ubjson(j, use_size, use_type); + } + + + /*! + @brief Serializes the given JSON object `j` to BSON and returns a vector + containing the corresponding BSON-representation. + + BSON (Binary JSON) is a binary format in which zero or more ordered key/value pairs are + stored as a single entity (a so-called document). + + The library uses the following mapping from JSON values types to BSON types: + + JSON value type | value/range | BSON type | marker + --------------- | --------------------------------- | ----------- | ------ + null | `null` | null | 0x0A + boolean | `true`, `false` | boolean | 0x08 + number_integer | -9223372036854775808..-2147483649 | int64 | 0x12 + number_integer | -2147483648..2147483647 | int32 | 0x10 + number_integer | 2147483648..9223372036854775807 | int64 | 0x12 + number_unsigned | 0..2147483647 | int32 | 0x10 + number_unsigned | 2147483648..9223372036854775807 | int64 | 0x12 + number_unsigned | 9223372036854775808..18446744073709551615| -- | -- + number_float | *any value* | double | 0x01 + string | *any value* | string | 0x02 + array | *any value* | document | 0x04 + object | *any value* | document | 0x03 + + @warning The mapping is **incomplete**, since only JSON-objects (and things + contained therein) can be serialized to BSON. + Also, integers larger than 9223372036854775807 cannot be serialized to BSON, + and the keys may not contain U+0000, since they are serialized a + zero-terminated c-strings. + + @throw out_of_range.407 if `j.is_number_unsigned() && j.get<std::uint64_t>() > 9223372036854775807` + @throw out_of_range.409 if a key in `j` contains a NULL (U+0000) + @throw type_error.317 if `!j.is_object()` + + @pre The input `j` is required to be an object: `j.is_object() == true`. + + @note Any BSON output created via @ref to_bson can be successfully parsed + by @ref from_bson. + + @param[in] j JSON value to serialize + @return BSON serialization as byte vector + + @complexity Linear in the size of the JSON value @a j. + + @liveexample{The example shows the serialization of a JSON value to a byte + vector in BSON format.,to_bson} + + @sa http://bsonspec.org/spec.html + @sa @ref from_bson(detail::input_adapter&&, const bool strict) for the + analogous deserialization + @sa @ref to_ubjson(const basic_json&, const bool, const bool) for the + related UBJSON format + @sa @ref to_cbor(const basic_json&) for the related CBOR format + @sa @ref to_msgpack(const basic_json&) for the related MessagePack format + */ + static std::vector<uint8_t> to_bson(const basic_json& j) + { + std::vector<uint8_t> result; + to_bson(j, result); + return result; + } + + /*! + @brief Serializes the given JSON object `j` to BSON and forwards the + corresponding BSON-representation to the given output_adapter `o`. + @param j The JSON object to convert to BSON. + @param o The output adapter that receives the binary BSON representation. + @pre The input `j` shall be an object: `j.is_object() == true` + @sa @ref to_bson(const basic_json&) + */ + static void to_bson(const basic_json& j, detail::output_adapter<uint8_t> o) + { + binary_writer<uint8_t>(o).write_bson(j); + } + + /*! + @copydoc to_bson(const basic_json&, detail::output_adapter<uint8_t>) + */ + static void to_bson(const basic_json& j, detail::output_adapter<char> o) + { + binary_writer<char>(o).write_bson(j); + } + + + /*! + @brief create a JSON value from an input in CBOR format + + Deserializes a given input @a i to a JSON value using the CBOR (Concise + Binary Object Representation) serialization format. + + The library maps CBOR types to JSON value types as follows: + + CBOR type | JSON value type | first byte + ---------------------- | --------------- | ---------- + Integer | number_unsigned | 0x00..0x17 + Unsigned integer | number_unsigned | 0x18 + Unsigned integer | number_unsigned | 0x19 + Unsigned integer | number_unsigned | 0x1A + Unsigned integer | number_unsigned | 0x1B + Negative integer | number_integer | 0x20..0x37 + Negative integer | number_integer | 0x38 + Negative integer | number_integer | 0x39 + Negative integer | number_integer | 0x3A + Negative integer | number_integer | 0x3B + Negative integer | number_integer | 0x40..0x57 + UTF-8 string | string | 0x60..0x77 + UTF-8 string | string | 0x78 + UTF-8 string | string | 0x79 + UTF-8 string | string | 0x7A + UTF-8 string | string | 0x7B + UTF-8 string | string | 0x7F + array | array | 0x80..0x97 + array | array | 0x98 + array | array | 0x99 + array | array | 0x9A + array | array | 0x9B + array | array | 0x9F + map | object | 0xA0..0xB7 + map | object | 0xB8 + map | object | 0xB9 + map | object | 0xBA + map | object | 0xBB + map | object | 0xBF + False | `false` | 0xF4 + True | `true` | 0xF5 + Null | `null` | 0xF6 + Half-Precision Float | number_float | 0xF9 + Single-Precision Float | number_float | 0xFA + Double-Precision Float | number_float | 0xFB + + @warning The mapping is **incomplete** in the sense that not all CBOR + types can be converted to a JSON value. The following CBOR types + are not supported and will yield parse errors (parse_error.112): + - byte strings (0x40..0x5F) + - date/time (0xC0..0xC1) + - bignum (0xC2..0xC3) + - decimal fraction (0xC4) + - bigfloat (0xC5) + - tagged items (0xC6..0xD4, 0xD8..0xDB) + - expected conversions (0xD5..0xD7) + - simple values (0xE0..0xF3, 0xF8) + - undefined (0xF7) + + @warning CBOR allows map keys of any type, whereas JSON only allows + strings as keys in object values. Therefore, CBOR maps with keys + other than UTF-8 strings are rejected (parse_error.113). + + @note Any CBOR output created @ref to_cbor can be successfully parsed by + @ref from_cbor. + + @param[in] i an input in CBOR format convertible to an input adapter + @param[in] strict whether to expect the input to be consumed until EOF + (true by default) + @param[in] allow_exceptions whether to throw exceptions in case of a + parse error (optional, true by default) + + @return deserialized JSON value + + @throw parse_error.110 if the given input ends prematurely or the end of + file was not reached when @a strict was set to true + @throw parse_error.112 if unsupported features from CBOR were + used in the given input @a v or if the input is not valid CBOR + @throw parse_error.113 if a string was expected as map key, but not found + + @complexity Linear in the size of the input @a i. + + @liveexample{The example shows the deserialization of a byte vector in CBOR + format to a JSON value.,from_cbor} + + @sa http://cbor.io + @sa @ref to_cbor(const basic_json&) for the analogous serialization + @sa @ref from_msgpack(detail::input_adapter&&, const bool, const bool) for the + related MessagePack format + @sa @ref from_ubjson(detail::input_adapter&&, const bool, const bool) for the + related UBJSON format + + @since version 2.0.9; parameter @a start_index since 2.1.1; changed to + consume input adapters, removed start_index parameter, and added + @a strict parameter since 3.0.0; added @a allow_exceptions parameter + since 3.2.0 + */ + static basic_json from_cbor(detail::input_adapter&& i, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(i)).sax_parse(input_format_t::cbor, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @copydoc from_cbor(detail::input_adapter&&, const bool, const bool) + */ + template<typename A1, typename A2, + detail::enable_if_t<std::is_constructible<detail::input_adapter, A1, A2>::value, int> = 0> + static basic_json from_cbor(A1 && a1, A2 && a2, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(std::forward<A1>(a1), std::forward<A2>(a2))).sax_parse(input_format_t::cbor, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @brief create a JSON value from an input in MessagePack format + + Deserializes a given input @a i to a JSON value using the MessagePack + serialization format. + + The library maps MessagePack types to JSON value types as follows: + + MessagePack type | JSON value type | first byte + ---------------- | --------------- | ---------- + positive fixint | number_unsigned | 0x00..0x7F + fixmap | object | 0x80..0x8F + fixarray | array | 0x90..0x9F + fixstr | string | 0xA0..0xBF + nil | `null` | 0xC0 + false | `false` | 0xC2 + true | `true` | 0xC3 + float 32 | number_float | 0xCA + float 64 | number_float | 0xCB + uint 8 | number_unsigned | 0xCC + uint 16 | number_unsigned | 0xCD + uint 32 | number_unsigned | 0xCE + uint 64 | number_unsigned | 0xCF + int 8 | number_integer | 0xD0 + int 16 | number_integer | 0xD1 + int 32 | number_integer | 0xD2 + int 64 | number_integer | 0xD3 + str 8 | string | 0xD9 + str 16 | string | 0xDA + str 32 | string | 0xDB + array 16 | array | 0xDC + array 32 | array | 0xDD + map 16 | object | 0xDE + map 32 | object | 0xDF + negative fixint | number_integer | 0xE0-0xFF + + @warning The mapping is **incomplete** in the sense that not all + MessagePack types can be converted to a JSON value. The following + MessagePack types are not supported and will yield parse errors: + - bin 8 - bin 32 (0xC4..0xC6) + - ext 8 - ext 32 (0xC7..0xC9) + - fixext 1 - fixext 16 (0xD4..0xD8) + + @note Any MessagePack output created @ref to_msgpack can be successfully + parsed by @ref from_msgpack. + + @param[in] i an input in MessagePack format convertible to an input + adapter + @param[in] strict whether to expect the input to be consumed until EOF + (true by default) + @param[in] allow_exceptions whether to throw exceptions in case of a + parse error (optional, true by default) + + @return deserialized JSON value + + @throw parse_error.110 if the given input ends prematurely or the end of + file was not reached when @a strict was set to true + @throw parse_error.112 if unsupported features from MessagePack were + used in the given input @a i or if the input is not valid MessagePack + @throw parse_error.113 if a string was expected as map key, but not found + + @complexity Linear in the size of the input @a i. + + @liveexample{The example shows the deserialization of a byte vector in + MessagePack format to a JSON value.,from_msgpack} + + @sa http://msgpack.org + @sa @ref to_msgpack(const basic_json&) for the analogous serialization + @sa @ref from_cbor(detail::input_adapter&&, const bool, const bool) for the + related CBOR format + @sa @ref from_ubjson(detail::input_adapter&&, const bool, const bool) for + the related UBJSON format + @sa @ref from_bson(detail::input_adapter&&, const bool, const bool) for + the related BSON format + + @since version 2.0.9; parameter @a start_index since 2.1.1; changed to + consume input adapters, removed start_index parameter, and added + @a strict parameter since 3.0.0; added @a allow_exceptions parameter + since 3.2.0 + */ + static basic_json from_msgpack(detail::input_adapter&& i, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(i)).sax_parse(input_format_t::msgpack, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @copydoc from_msgpack(detail::input_adapter&&, const bool, const bool) + */ + template<typename A1, typename A2, + detail::enable_if_t<std::is_constructible<detail::input_adapter, A1, A2>::value, int> = 0> + static basic_json from_msgpack(A1 && a1, A2 && a2, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(std::forward<A1>(a1), std::forward<A2>(a2))).sax_parse(input_format_t::msgpack, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @brief create a JSON value from an input in UBJSON format + + Deserializes a given input @a i to a JSON value using the UBJSON (Universal + Binary JSON) serialization format. + + The library maps UBJSON types to JSON value types as follows: + + UBJSON type | JSON value type | marker + ----------- | --------------------------------------- | ------ + no-op | *no value, next value is read* | `N` + null | `null` | `Z` + false | `false` | `F` + true | `true` | `T` + float32 | number_float | `d` + float64 | number_float | `D` + uint8 | number_unsigned | `U` + int8 | number_integer | `i` + int16 | number_integer | `I` + int32 | number_integer | `l` + int64 | number_integer | `L` + string | string | `S` + char | string | `C` + array | array (optimized values are supported) | `[` + object | object (optimized values are supported) | `{` + + @note The mapping is **complete** in the sense that any UBJSON value can + be converted to a JSON value. + + @param[in] i an input in UBJSON format convertible to an input adapter + @param[in] strict whether to expect the input to be consumed until EOF + (true by default) + @param[in] allow_exceptions whether to throw exceptions in case of a + parse error (optional, true by default) + + @return deserialized JSON value + + @throw parse_error.110 if the given input ends prematurely or the end of + file was not reached when @a strict was set to true + @throw parse_error.112 if a parse error occurs + @throw parse_error.113 if a string could not be parsed successfully + + @complexity Linear in the size of the input @a i. + + @liveexample{The example shows the deserialization of a byte vector in + UBJSON format to a JSON value.,from_ubjson} + + @sa http://ubjson.org + @sa @ref to_ubjson(const basic_json&, const bool, const bool) for the + analogous serialization + @sa @ref from_cbor(detail::input_adapter&&, const bool, const bool) for the + related CBOR format + @sa @ref from_msgpack(detail::input_adapter&&, const bool, const bool) for + the related MessagePack format + @sa @ref from_bson(detail::input_adapter&&, const bool, const bool) for + the related BSON format + + @since version 3.1.0; added @a allow_exceptions parameter since 3.2.0 + */ + static basic_json from_ubjson(detail::input_adapter&& i, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(i)).sax_parse(input_format_t::ubjson, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @copydoc from_ubjson(detail::input_adapter&&, const bool, const bool) + */ + template<typename A1, typename A2, + detail::enable_if_t<std::is_constructible<detail::input_adapter, A1, A2>::value, int> = 0> + static basic_json from_ubjson(A1 && a1, A2 && a2, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(std::forward<A1>(a1), std::forward<A2>(a2))).sax_parse(input_format_t::ubjson, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @brief Create a JSON value from an input in BSON format + + Deserializes a given input @a i to a JSON value using the BSON (Binary JSON) + serialization format. + + The library maps BSON record types to JSON value types as follows: + + BSON type | BSON marker byte | JSON value type + --------------- | ---------------- | --------------------------- + double | 0x01 | number_float + string | 0x02 | string + document | 0x03 | object + array | 0x04 | array + binary | 0x05 | still unsupported + undefined | 0x06 | still unsupported + ObjectId | 0x07 | still unsupported + boolean | 0x08 | boolean + UTC Date-Time | 0x09 | still unsupported + null | 0x0A | null + Regular Expr. | 0x0B | still unsupported + DB Pointer | 0x0C | still unsupported + JavaScript Code | 0x0D | still unsupported + Symbol | 0x0E | still unsupported + JavaScript Code | 0x0F | still unsupported + int32 | 0x10 | number_integer + Timestamp | 0x11 | still unsupported + 128-bit decimal float | 0x13 | still unsupported + Max Key | 0x7F | still unsupported + Min Key | 0xFF | still unsupported + + @warning The mapping is **incomplete**. The unsupported mappings + are indicated in the table above. + + @param[in] i an input in BSON format convertible to an input adapter + @param[in] strict whether to expect the input to be consumed until EOF + (true by default) + @param[in] allow_exceptions whether to throw exceptions in case of a + parse error (optional, true by default) + + @return deserialized JSON value + + @throw parse_error.114 if an unsupported BSON record type is encountered + + @complexity Linear in the size of the input @a i. + + @liveexample{The example shows the deserialization of a byte vector in + BSON format to a JSON value.,from_bson} + + @sa http://bsonspec.org/spec.html + @sa @ref to_bson(const basic_json&) for the analogous serialization + @sa @ref from_cbor(detail::input_adapter&&, const bool, const bool) for the + related CBOR format + @sa @ref from_msgpack(detail::input_adapter&&, const bool, const bool) for + the related MessagePack format + @sa @ref from_ubjson(detail::input_adapter&&, const bool, const bool) for the + related UBJSON format + */ + static basic_json from_bson(detail::input_adapter&& i, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(i)).sax_parse(input_format_t::bson, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + /*! + @copydoc from_bson(detail::input_adapter&&, const bool, const bool) + */ + template<typename A1, typename A2, + detail::enable_if_t<std::is_constructible<detail::input_adapter, A1, A2>::value, int> = 0> + static basic_json from_bson(A1 && a1, A2 && a2, + const bool strict = true, + const bool allow_exceptions = true) + { + basic_json result; + detail::json_sax_dom_parser<basic_json> sdp(result, allow_exceptions); + const bool res = binary_reader(detail::input_adapter(std::forward<A1>(a1), std::forward<A2>(a2))).sax_parse(input_format_t::bson, &sdp, strict); + return res ? result : basic_json(value_t::discarded); + } + + + + /// @} + + ////////////////////////// + // JSON Pointer support // + ////////////////////////// + + /// @name JSON Pointer functions + /// @{ + + /*! + @brief access specified element via JSON Pointer + + Uses a JSON pointer to retrieve a reference to the respective JSON value. + No bound checking is performed. Similar to @ref operator[](const typename + object_t::key_type&), `null` values are created in arrays and objects if + necessary. + + In particular: + - If the JSON pointer points to an object key that does not exist, it + is created an filled with a `null` value before a reference to it + is returned. + - If the JSON pointer points to an array index that does not exist, it + is created an filled with a `null` value before a reference to it + is returned. All indices between the current maximum and the given + index are also filled with `null`. + - The special value `-` is treated as a synonym for the index past the + end. + + @param[in] ptr a JSON pointer + + @return reference to the element pointed to by @a ptr + + @complexity Constant. + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.404 if the JSON pointer can not be resolved + + @liveexample{The behavior is shown in the example.,operatorjson_pointer} + + @since version 2.0.0 + */ + reference operator[](const json_pointer& ptr) + { + return ptr.get_unchecked(this); + } + + /*! + @brief access specified element via JSON Pointer + + Uses a JSON pointer to retrieve a reference to the respective JSON value. + No bound checking is performed. The function does not change the JSON + value; no `null` values are created. In particular, the the special value + `-` yields an exception. + + @param[in] ptr JSON pointer to the desired element + + @return const reference to the element pointed to by @a ptr + + @complexity Constant. + + @throw parse_error.106 if an array index begins with '0' + @throw parse_error.109 if an array index was not a number + @throw out_of_range.402 if the array index '-' is used + @throw out_of_range.404 if the JSON pointer can not be resolved + + @liveexample{The behavior is shown in the example.,operatorjson_pointer_const} + + @since version 2.0.0 + */ + const_reference operator[](const json_pointer& ptr) const + { + return ptr.get_unchecked(this); + } + + /*! + @brief access specified element via JSON Pointer + + Returns a reference to the element at with specified JSON pointer @a ptr, + with bounds checking. + + @param[in] ptr JSON pointer to the desired element + + @return reference to the element pointed to by @a ptr + + @throw parse_error.106 if an array index in the passed JSON pointer @a ptr + begins with '0'. See example below. + + @throw parse_error.109 if an array index in the passed JSON pointer @a ptr + is not a number. See example below. + + @throw out_of_range.401 if an array index in the passed JSON pointer @a ptr + is out of range. See example below. + + @throw out_of_range.402 if the array index '-' is used in the passed JSON + pointer @a ptr. As `at` provides checked access (and no elements are + implicitly inserted), the index '-' is always invalid. See example below. + + @throw out_of_range.403 if the JSON pointer describes a key of an object + which cannot be found. See example below. + + @throw out_of_range.404 if the JSON pointer @a ptr can not be resolved. + See example below. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Constant. + + @since version 2.0.0 + + @liveexample{The behavior is shown in the example.,at_json_pointer} + */ + reference at(const json_pointer& ptr) + { + return ptr.get_checked(this); + } + + /*! + @brief access specified element via JSON Pointer + + Returns a const reference to the element at with specified JSON pointer @a + ptr, with bounds checking. + + @param[in] ptr JSON pointer to the desired element + + @return reference to the element pointed to by @a ptr + + @throw parse_error.106 if an array index in the passed JSON pointer @a ptr + begins with '0'. See example below. + + @throw parse_error.109 if an array index in the passed JSON pointer @a ptr + is not a number. See example below. + + @throw out_of_range.401 if an array index in the passed JSON pointer @a ptr + is out of range. See example below. + + @throw out_of_range.402 if the array index '-' is used in the passed JSON + pointer @a ptr. As `at` provides checked access (and no elements are + implicitly inserted), the index '-' is always invalid. See example below. + + @throw out_of_range.403 if the JSON pointer describes a key of an object + which cannot be found. See example below. + + @throw out_of_range.404 if the JSON pointer @a ptr can not be resolved. + See example below. + + @exceptionsafety Strong guarantee: if an exception is thrown, there are no + changes in the JSON value. + + @complexity Constant. + + @since version 2.0.0 + + @liveexample{The behavior is shown in the example.,at_json_pointer_const} + */ + const_reference at(const json_pointer& ptr) const + { + return ptr.get_checked(this); + } + + /*! + @brief return flattened JSON value + + The function creates a JSON object whose keys are JSON pointers (see [RFC + 6901](https://tools.ietf.org/html/rfc6901)) and whose values are all + primitive. The original JSON value can be restored using the @ref + unflatten() function. + + @return an object that maps JSON pointers to primitive values + + @note Empty objects and arrays are flattened to `null` and will not be + reconstructed correctly by the @ref unflatten() function. + + @complexity Linear in the size the JSON value. + + @liveexample{The following code shows how a JSON object is flattened to an + object whose keys consist of JSON pointers.,flatten} + + @sa @ref unflatten() for the reverse function + + @since version 2.0.0 + */ + basic_json flatten() const + { + basic_json result(value_t::object); + json_pointer::flatten("", *this, result); + return result; + } + + /*! + @brief unflatten a previously flattened JSON value + + The function restores the arbitrary nesting of a JSON value that has been + flattened before using the @ref flatten() function. The JSON value must + meet certain constraints: + 1. The value must be an object. + 2. The keys must be JSON pointers (see + [RFC 6901](https://tools.ietf.org/html/rfc6901)) + 3. The mapped values must be primitive JSON types. + + @return the original JSON from a flattened version + + @note Empty objects and arrays are flattened by @ref flatten() to `null` + values and can not unflattened to their original type. Apart from + this example, for a JSON value `j`, the following is always true: + `j == j.flatten().unflatten()`. + + @complexity Linear in the size the JSON value. + + @throw type_error.314 if value is not an object + @throw type_error.315 if object values are not primitive + + @liveexample{The following code shows how a flattened JSON object is + unflattened into the original nested JSON object.,unflatten} + + @sa @ref flatten() for the reverse function + + @since version 2.0.0 + */ + basic_json unflatten() const + { + return json_pointer::unflatten(*this); + } + + /// @} + + ////////////////////////// + // JSON Patch functions // + ////////////////////////// + + /// @name JSON Patch functions + /// @{ + + /*! + @brief applies a JSON patch + + [JSON Patch](http://jsonpatch.com) defines a JSON document structure for + expressing a sequence of operations to apply to a JSON) document. With + this function, a JSON Patch is applied to the current JSON value by + executing all operations from the patch. + + @param[in] json_patch JSON patch document + @return patched document + + @note The application of a patch is atomic: Either all operations succeed + and the patched document is returned or an exception is thrown. In + any case, the original value is not changed: the patch is applied + to a copy of the value. + + @throw parse_error.104 if the JSON patch does not consist of an array of + objects + + @throw parse_error.105 if the JSON patch is malformed (e.g., mandatory + attributes are missing); example: `"operation add must have member path"` + + @throw out_of_range.401 if an array index is out of range. + + @throw out_of_range.403 if a JSON pointer inside the patch could not be + resolved successfully in the current JSON value; example: `"key baz not + found"` + + @throw out_of_range.405 if JSON pointer has no parent ("add", "remove", + "move") + + @throw other_error.501 if "test" operation was unsuccessful + + @complexity Linear in the size of the JSON value and the length of the + JSON patch. As usually only a fraction of the JSON value is affected by + the patch, the complexity can usually be neglected. + + @liveexample{The following code shows how a JSON patch is applied to a + value.,patch} + + @sa @ref diff -- create a JSON patch by comparing two JSON values + + @sa [RFC 6902 (JSON Patch)](https://tools.ietf.org/html/rfc6902) + @sa [RFC 6901 (JSON Pointer)](https://tools.ietf.org/html/rfc6901) + + @since version 2.0.0 + */ + basic_json patch(const basic_json& json_patch) const + { + // make a working copy to apply the patch to + basic_json result = *this; + + // the valid JSON Patch operations + enum class patch_operations {add, remove, replace, move, copy, test, invalid}; + + const auto get_op = [](const std::string & op) + { + if (op == "add") + { + return patch_operations::add; + } + if (op == "remove") + { + return patch_operations::remove; + } + if (op == "replace") + { + return patch_operations::replace; + } + if (op == "move") + { + return patch_operations::move; + } + if (op == "copy") + { + return patch_operations::copy; + } + if (op == "test") + { + return patch_operations::test; + } + + return patch_operations::invalid; + }; + + // wrapper for "add" operation; add value at ptr + const auto operation_add = [&result](json_pointer & ptr, basic_json val) + { + // adding to the root of the target document means replacing it + if (ptr.is_root()) + { + result = val; + } + else + { + // make sure the top element of the pointer exists + json_pointer top_pointer = ptr.top(); + if (top_pointer != ptr) + { + result.at(top_pointer); + } + + // get reference to parent of JSON pointer ptr + const auto last_path = ptr.pop_back(); + basic_json& parent = result[ptr]; + + switch (parent.m_type) + { + case value_t::null: + case value_t::object: + { + // use operator[] to add value + parent[last_path] = val; + break; + } + + case value_t::array: + { + if (last_path == "-") + { + // special case: append to back + parent.push_back(val); + } + else + { + const auto idx = json_pointer::array_index(last_path); + if (JSON_UNLIKELY(static_cast<size_type>(idx) > parent.size())) + { + // avoid undefined behavior + JSON_THROW(out_of_range::create(401, "array index " + std::to_string(idx) + " is out of range")); + } + + // default case: insert add offset + parent.insert(parent.begin() + static_cast<difference_type>(idx), val); + } + break; + } + + // LCOV_EXCL_START + default: + { + // if there exists a parent it cannot be primitive + assert(false); + } + // LCOV_EXCL_STOP + } + } + }; + + // wrapper for "remove" operation; remove value at ptr + const auto operation_remove = [&result](json_pointer & ptr) + { + // get reference to parent of JSON pointer ptr + const auto last_path = ptr.pop_back(); + basic_json& parent = result.at(ptr); + + // remove child + if (parent.is_object()) + { + // perform range check + auto it = parent.find(last_path); + if (JSON_LIKELY(it != parent.end())) + { + parent.erase(it); + } + else + { + JSON_THROW(out_of_range::create(403, "key '" + last_path + "' not found")); + } + } + else if (parent.is_array()) + { + // note erase performs range check + parent.erase(static_cast<size_type>(json_pointer::array_index(last_path))); + } + }; + + // type check: top level value must be an array + if (JSON_UNLIKELY(not json_patch.is_array())) + { + JSON_THROW(parse_error::create(104, 0, "JSON patch must be an array of objects")); + } + + // iterate and apply the operations + for (const auto& val : json_patch) + { + // wrapper to get a value for an operation + const auto get_value = [&val](const std::string & op, + const std::string & member, + bool string_type) -> basic_json & + { + // find value + auto it = val.m_value.object->find(member); + + // context-sensitive error message + const auto error_msg = (op == "op") ? "operation" : "operation '" + op + "'"; + + // check if desired value is present + if (JSON_UNLIKELY(it == val.m_value.object->end())) + { + JSON_THROW(parse_error::create(105, 0, error_msg + " must have member '" + member + "'")); + } + + // check if result is of type string + if (JSON_UNLIKELY(string_type and not it->second.is_string())) + { + JSON_THROW(parse_error::create(105, 0, error_msg + " must have string member '" + member + "'")); + } + + // no error: return value + return it->second; + }; + + // type check: every element of the array must be an object + if (JSON_UNLIKELY(not val.is_object())) + { + JSON_THROW(parse_error::create(104, 0, "JSON patch must be an array of objects")); + } + + // collect mandatory members + const std::string op = get_value("op", "op", true); + const std::string path = get_value(op, "path", true); + json_pointer ptr(path); + + switch (get_op(op)) + { + case patch_operations::add: + { + operation_add(ptr, get_value("add", "value", false)); + break; + } + + case patch_operations::remove: + { + operation_remove(ptr); + break; + } + + case patch_operations::replace: + { + // the "path" location must exist - use at() + result.at(ptr) = get_value("replace", "value", false); + break; + } + + case patch_operations::move: + { + const std::string from_path = get_value("move", "from", true); + json_pointer from_ptr(from_path); + + // the "from" location must exist - use at() + basic_json v = result.at(from_ptr); + + // The move operation is functionally identical to a + // "remove" operation on the "from" location, followed + // immediately by an "add" operation at the target + // location with the value that was just removed. + operation_remove(from_ptr); + operation_add(ptr, v); + break; + } + + case patch_operations::copy: + { + const std::string from_path = get_value("copy", "from", true); + const json_pointer from_ptr(from_path); + + // the "from" location must exist - use at() + basic_json v = result.at(from_ptr); + + // The copy is functionally identical to an "add" + // operation at the target location using the value + // specified in the "from" member. + operation_add(ptr, v); + break; + } + + case patch_operations::test: + { + bool success = false; + JSON_TRY + { + // check if "value" matches the one at "path" + // the "path" location must exist - use at() + success = (result.at(ptr) == get_value("test", "value", false)); + } + JSON_INTERNAL_CATCH (out_of_range&) + { + // ignore out of range errors: success remains false + } + + // throw an exception if test fails + if (JSON_UNLIKELY(not success)) + { + JSON_THROW(other_error::create(501, "unsuccessful: " + val.dump())); + } + + break; + } + + case patch_operations::invalid: + { + // op must be "add", "remove", "replace", "move", "copy", or + // "test" + JSON_THROW(parse_error::create(105, 0, "operation value '" + op + "' is invalid")); + } + } + } + + return result; + } + + /*! + @brief creates a diff as a JSON patch + + Creates a [JSON Patch](http://jsonpatch.com) so that value @a source can + be changed into the value @a target by calling @ref patch function. + + @invariant For two JSON values @a source and @a target, the following code + yields always `true`: + @code {.cpp} + source.patch(diff(source, target)) == target; + @endcode + + @note Currently, only `remove`, `add`, and `replace` operations are + generated. + + @param[in] source JSON value to compare from + @param[in] target JSON value to compare against + @param[in] path helper value to create JSON pointers + + @return a JSON patch to convert the @a source to @a target + + @complexity Linear in the lengths of @a source and @a target. + + @liveexample{The following code shows how a JSON patch is created as a + diff for two JSON values.,diff} + + @sa @ref patch -- apply a JSON patch + @sa @ref merge_patch -- apply a JSON Merge Patch + + @sa [RFC 6902 (JSON Patch)](https://tools.ietf.org/html/rfc6902) + + @since version 2.0.0 + */ + static basic_json diff(const basic_json& source, const basic_json& target, + const std::string& path = "") + { + // the patch + basic_json result(value_t::array); + + // if the values are the same, return empty patch + if (source == target) + { + return result; + } + + if (source.type() != target.type()) + { + // different types: replace value + result.push_back( + { + {"op", "replace"}, {"path", path}, {"value", target} + }); + } + else + { + switch (source.type()) + { + case value_t::array: + { + // first pass: traverse common elements + std::size_t i = 0; + while (i < source.size() and i < target.size()) + { + // recursive call to compare array values at index i + auto temp_diff = diff(source[i], target[i], path + "/" + std::to_string(i)); + result.insert(result.end(), temp_diff.begin(), temp_diff.end()); + ++i; + } + + // i now reached the end of at least one array + // in a second pass, traverse the remaining elements + + // remove my remaining elements + const auto end_index = static_cast<difference_type>(result.size()); + while (i < source.size()) + { + // add operations in reverse order to avoid invalid + // indices + result.insert(result.begin() + end_index, object( + { + {"op", "remove"}, + {"path", path + "/" + std::to_string(i)} + })); + ++i; + } + + // add other remaining elements + while (i < target.size()) + { + result.push_back( + { + {"op", "add"}, + {"path", path + "/" + std::to_string(i)}, + {"value", target[i]} + }); + ++i; + } + + break; + } + + case value_t::object: + { + // first pass: traverse this object's elements + for (auto it = source.cbegin(); it != source.cend(); ++it) + { + // escape the key name to be used in a JSON patch + const auto key = json_pointer::escape(it.key()); + + if (target.find(it.key()) != target.end()) + { + // recursive call to compare object values at key it + auto temp_diff = diff(it.value(), target[it.key()], path + "/" + key); + result.insert(result.end(), temp_diff.begin(), temp_diff.end()); + } + else + { + // found a key that is not in o -> remove it + result.push_back(object( + { + {"op", "remove"}, {"path", path + "/" + key} + })); + } + } + + // second pass: traverse other object's elements + for (auto it = target.cbegin(); it != target.cend(); ++it) + { + if (source.find(it.key()) == source.end()) + { + // found a key that is not in this -> add it + const auto key = json_pointer::escape(it.key()); + result.push_back( + { + {"op", "add"}, {"path", path + "/" + key}, + {"value", it.value()} + }); + } + } + + break; + } + + default: + { + // both primitive type: replace value + result.push_back( + { + {"op", "replace"}, {"path", path}, {"value", target} + }); + break; + } + } + } + + return result; + } + + /// @} + + //////////////////////////////// + // JSON Merge Patch functions // + //////////////////////////////// + + /// @name JSON Merge Patch functions + /// @{ + + /*! + @brief applies a JSON Merge Patch + + The merge patch format is primarily intended for use with the HTTP PATCH + method as a means of describing a set of modifications to a target + resource's content. This function applies a merge patch to the current + JSON value. + + The function implements the following algorithm from Section 2 of + [RFC 7396 (JSON Merge Patch)](https://tools.ietf.org/html/rfc7396): + + ``` + define MergePatch(Target, Patch): + if Patch is an Object: + if Target is not an Object: + Target = {} // Ignore the contents and set it to an empty Object + for each Name/Value pair in Patch: + if Value is null: + if Name exists in Target: + remove the Name/Value pair from Target + else: + Target[Name] = MergePatch(Target[Name], Value) + return Target + else: + return Patch + ``` + + Thereby, `Target` is the current object; that is, the patch is applied to + the current value. + + @param[in] patch the patch to apply + + @complexity Linear in the lengths of @a patch. + + @liveexample{The following code shows how a JSON Merge Patch is applied to + a JSON document.,merge_patch} + + @sa @ref patch -- apply a JSON patch + @sa [RFC 7396 (JSON Merge Patch)](https://tools.ietf.org/html/rfc7396) + + @since version 3.0.0 + */ + void merge_patch(const basic_json& patch) + { + if (patch.is_object()) + { + if (not is_object()) + { + *this = object(); + } + for (auto it = patch.begin(); it != patch.end(); ++it) + { + if (it.value().is_null()) + { + erase(it.key()); + } + else + { + operator[](it.key()).merge_patch(it.value()); + } + } + } + else + { + *this = patch; + } + } + + /// @} +}; +} // namespace nlohmann + +/////////////////////// +// nonmember support // +/////////////////////// + +// specialization of std::swap, and std::hash +namespace std +{ + +/// hash value for JSON objects +template<> +struct hash<nlohmann::json> +{ + /*! + @brief return a hash value for a JSON object + + @since version 1.0.0 + */ + std::size_t operator()(const nlohmann::json& j) const + { + // a naive hashing via the string representation + const auto& h = hash<nlohmann::json::string_t>(); + return h(j.dump()); + } +}; + +/// specialization for std::less<value_t> +/// @note: do not remove the space after '<', +/// see https://github.com/nlohmann/json/pull/679 +template<> +struct less< ::nlohmann::detail::value_t> +{ + /*! + @brief compare two value_t enum values + @since version 3.0.0 + */ + bool operator()(nlohmann::detail::value_t lhs, + nlohmann::detail::value_t rhs) const noexcept + { + return nlohmann::detail::operator<(lhs, rhs); + } +}; + +/*! +@brief exchanges the values of two JSON objects + +@since version 1.0.0 +*/ +template<> +inline void swap<nlohmann::json>(nlohmann::json& j1, nlohmann::json& j2) noexcept( + is_nothrow_move_constructible<nlohmann::json>::value and + is_nothrow_move_assignable<nlohmann::json>::value +) +{ + j1.swap(j2); +} + +} // namespace std + +/*! +@brief user-defined string literal for JSON values + +This operator implements a user-defined string literal for JSON objects. It +can be used by adding `"_json"` to a string literal and returns a JSON object +if no parse error occurred. + +@param[in] s a string representation of a JSON object +@param[in] n the length of string @a s +@return a JSON object + +@since version 1.0.0 +*/ +inline nlohmann::json operator "" _json(const char* s, std::size_t n) +{ + return nlohmann::json::parse(s, s + n); +} + +/*! +@brief user-defined string literal for JSON pointer + +This operator implements a user-defined string literal for JSON Pointers. It +can be used by adding `"_json_pointer"` to a string literal and returns a JSON pointer +object if no parse error occurred. + +@param[in] s a string representation of a JSON Pointer +@param[in] n the length of string @a s +@return a JSON pointer object + +@since version 2.0.0 +*/ +inline nlohmann::json::json_pointer operator "" _json_pointer(const char* s, std::size_t n) +{ + return nlohmann::json::json_pointer(std::string(s, n)); +} + +// #include <nlohmann/detail/macro_unscope.hpp> + + +// restore GCC/clang diagnostic settings +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #pragma GCC diagnostic pop +#endif +#if defined(__clang__) + #pragma GCC diagnostic pop +#endif + +// clean up +#undef JSON_INTERNAL_CATCH +#undef JSON_CATCH +#undef JSON_THROW +#undef JSON_TRY +#undef JSON_LIKELY +#undef JSON_UNLIKELY +#undef JSON_DEPRECATED +#undef JSON_HAS_CPP_14 +#undef JSON_HAS_CPP_17 +#undef NLOHMANN_BASIC_JSON_TPL_DECLARATION +#undef NLOHMANN_BASIC_JSON_TPL + + +#endif diff --git a/VirtualRobot/Util/xml/tinyxml2.cpp b/VirtualRobot/Util/xml/tinyxml2.cpp new file mode 100644 index 0000000000000000000000000000000000000000..69760a4909a539eca5da6ff4aba54c6a21508885 --- /dev/null +++ b/VirtualRobot/Util/xml/tinyxml2.cpp @@ -0,0 +1,2834 @@ +/* +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#include "tinyxml2.h" + +#include <new> // yes, this one new style header, is in the Android SDK. +#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__) +# include <stddef.h> +# include <stdarg.h> +#else +# include <cstddef> +# include <cstdarg> +#endif + +#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) && (!defined WINCE) + // Microsoft Visual Studio, version 2005 and higher. Not WinCE. + /*int _snprintf_s( + char *buffer, + size_t sizeOfBuffer, + size_t count, + const char *format [, + argument] ... + );*/ + static inline int TIXML_SNPRINTF( char* buffer, size_t size, const char* format, ... ) + { + va_list va; + va_start( va, format ); + int result = vsnprintf_s( buffer, size, _TRUNCATE, format, va ); + va_end( va ); + return result; + } + + static inline int TIXML_VSNPRINTF( char* buffer, size_t size, const char* format, va_list va ) + { + int result = vsnprintf_s( buffer, size, _TRUNCATE, format, va ); + return result; + } + + #define TIXML_VSCPRINTF _vscprintf + #define TIXML_SSCANF sscanf_s +#elif defined _MSC_VER + // Microsoft Visual Studio 2003 and earlier or WinCE + #define TIXML_SNPRINTF _snprintf + #define TIXML_VSNPRINTF _vsnprintf + #define TIXML_SSCANF sscanf + #if (_MSC_VER < 1400 ) && (!defined WINCE) + // Microsoft Visual Studio 2003 and not WinCE. + #define TIXML_VSCPRINTF _vscprintf // VS2003's C runtime has this, but VC6 C runtime or WinCE SDK doesn't have. + #else + // Microsoft Visual Studio 2003 and earlier or WinCE. + static inline int TIXML_VSCPRINTF( const char* format, va_list va ) + { + int len = 512; + for (;;) { + len = len*2; + char* str = new char[len](); + const int required = _vsnprintf(str, len, format, va); + delete[] str; + if ( required != -1 ) { + TIXMLASSERT( required >= 0 ); + len = required; + break; + } + } + TIXMLASSERT( len >= 0 ); + return len; + } + #endif +#else + // GCC version 3 and higher + //#warning( "Using sn* functions." ) + #define TIXML_SNPRINTF snprintf + #define TIXML_VSNPRINTF vsnprintf + static inline int TIXML_VSCPRINTF( const char* format, va_list va ) + { + int len = vsnprintf( nullptr, 0, format, va ); + TIXMLASSERT( len >= 0 ); + return len; + } + #define TIXML_SSCANF sscanf +#endif + + +static const char LINE_FEED = (char)0x0a; // all line endings are normalized to LF +static const char LF = LINE_FEED; +static const char CARRIAGE_RETURN = (char)0x0d; // CR gets filtered out +static const char CR = CARRIAGE_RETURN; +static const char SINGLE_QUOTE = '\''; +static const char DOUBLE_QUOTE = '\"'; + +// Bunch of unicode info at: +// http://www.unicode.org/faq/utf_bom.html +// ef bb bf (Microsoft "lead bytes") - designates UTF-8 + +static const unsigned char TIXML_UTF_LEAD_0 = 0xefU; +static const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; +static const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; + +namespace tinyxml2 +{ + +struct Entity { + const char* pattern; + int length; + char value; +}; + +static const int NUM_ENTITIES = 5; +static const Entity entities[NUM_ENTITIES] = { + { "quot", 4, DOUBLE_QUOTE }, + { "amp", 3, '&' }, + { "apos", 4, SINGLE_QUOTE }, + { "lt", 2, '<' }, + { "gt", 2, '>' } +}; + + +StrPair::~StrPair() +{ + Reset(); +} + + +void StrPair::TransferTo( StrPair* other ) +{ + if ( this == other ) { + return; + } + // This in effect implements the assignment operator by "moving" + // ownership (as in auto_ptr). + + TIXMLASSERT( other != nullptr ); + TIXMLASSERT( other->_flags == 0 ); + TIXMLASSERT( other->_start == nullptr ); + TIXMLASSERT( other->_end == nullptr ); + + other->Reset(); + + other->_flags = _flags; + other->_start = _start; + other->_end = _end; + + _flags = 0; + _start = nullptr; + _end = nullptr; +} + + +void StrPair::Reset() +{ + if ( _flags & NEEDS_DELETE ) { + delete [] _start; + } + _flags = 0; + _start = nullptr; + _end = nullptr; +} + + +void StrPair::SetStr( const char* str, int flags ) +{ + TIXMLASSERT( str ); + Reset(); + size_t len = strlen( str ); + TIXMLASSERT( _start == nullptr ); + _start = new char[ len+1 ]; + memcpy( _start, str, len+1 ); + _end = _start + len; + _flags = flags | NEEDS_DELETE; +} + + +char* StrPair::ParseText( char* p, const char* endTag, int strFlags, int* curLineNumPtr ) +{ + TIXMLASSERT( p ); + TIXMLASSERT( endTag && *endTag ); + TIXMLASSERT(curLineNumPtr); + + char* start = p; + char endChar = *endTag; + size_t length = strlen( endTag ); + + // Inner loop of text parsing. + while ( *p ) { + if ( *p == endChar && strncmp( p, endTag, length ) == 0 ) { + Set( start, p, strFlags ); + return p + length; + } else if (*p == '\n') { + ++(*curLineNumPtr); + } + ++p; + TIXMLASSERT( p ); + } + return nullptr; +} + + +char* StrPair::ParseName( char* p ) +{ + if ( !p || !(*p) ) { + return nullptr; + } + if ( !XMLUtil::IsNameStartChar( *p ) ) { + return nullptr; + } + + char* const start = p; + ++p; + while ( *p && XMLUtil::IsNameChar( *p ) ) { + ++p; + } + + Set( start, p, 0 ); + return p; +} + + +void StrPair::CollapseWhitespace() +{ + // Adjusting _start would cause undefined behavior on delete[] + TIXMLASSERT( ( _flags & NEEDS_DELETE ) == 0 ); + // Trim leading space. + _start = XMLUtil::SkipWhiteSpace( _start, nullptr ); + + if ( *_start ) { + const char* p = _start; // the read pointer + char* q = _start; // the write pointer + + while( *p ) { + if ( XMLUtil::IsWhiteSpace( *p )) { + p = XMLUtil::SkipWhiteSpace( p, nullptr ); + if ( *p == 0 ) { + break; // don't write to q; this trims the trailing space. + } + *q = ' '; + ++q; + } + *q = *p; + ++q; + ++p; + } + *q = 0; + } +} + + +const char* StrPair::GetStr() +{ + TIXMLASSERT( _start ); + TIXMLASSERT( _end ); + if ( _flags & NEEDS_FLUSH ) { + *_end = 0; + _flags ^= NEEDS_FLUSH; + + if ( _flags ) { + const char* p = _start; // the read pointer + char* q = _start; // the write pointer + + while( p < _end ) { + if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == CR ) { + // CR-LF pair becomes LF + // CR alone becomes LF + // LF-CR becomes LF + if ( *(p+1) == LF ) { + p += 2; + } + else { + ++p; + } + *q = LF; + ++q; + } + else if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == LF ) { + if ( *(p+1) == CR ) { + p += 2; + } + else { + ++p; + } + *q = LF; + ++q; + } + else if ( (_flags & NEEDS_ENTITY_PROCESSING) && *p == '&' ) { + // Entities handled by tinyXML2: + // - special entities in the entity table [in/out] + // - numeric character reference [in] + // 中 or 中 + + if ( *(p+1) == '#' ) { + const int buflen = 10; + char buf[buflen] = { 0 }; + int len = 0; + char* adjusted = const_cast<char*>( XMLUtil::GetCharacterRef( p, buf, &len ) ); + if ( adjusted == nullptr ) { + *q = *p; + ++p; + ++q; + } + else { + TIXMLASSERT( 0 <= len && len <= buflen ); + TIXMLASSERT( q + len <= adjusted ); + p = adjusted; + memcpy( q, buf, len ); + q += len; + } + } + else { + bool entityFound = false; + for(auto entity : entities) { + if ( strncmp( p + 1, entity.pattern, entity.length ) == 0 + && *( p + entity.length + 1 ) == ';' ) { + // Found an entity - convert. + *q = entity.value; + ++q; + p += entity.length + 2; + entityFound = true; + break; + } + } + if ( !entityFound ) { + // fixme: treat as error? + ++p; + ++q; + } + } + } + else { + *q = *p; + ++p; + ++q; + } + } + *q = 0; + } + // The loop below has plenty going on, and this + // is a less useful mode. Break it out. + if ( _flags & NEEDS_WHITESPACE_COLLAPSING ) { + CollapseWhitespace(); + } + _flags = (_flags & NEEDS_DELETE); + } + TIXMLASSERT( _start ); + return _start; +} + + + + +// --------- XMLUtil ----------- // + +const char* XMLUtil::writeBoolTrue = "true"; +const char* XMLUtil::writeBoolFalse = "false"; + +void XMLUtil::SetBoolSerialization(const char* writeTrue, const char* writeFalse) +{ + static const char* defTrue = "true"; + static const char* defFalse = "false"; + + writeBoolTrue = (writeTrue) ? writeTrue : defTrue; + writeBoolFalse = (writeFalse) ? writeFalse : defFalse; +} + + +const char* XMLUtil::ReadBOM( const char* p, bool* bom ) +{ + TIXMLASSERT( p ); + TIXMLASSERT( bom ); + *bom = false; + const unsigned char* pu = reinterpret_cast<const unsigned char*>(p); + // Check for BOM: + if ( *(pu+0) == TIXML_UTF_LEAD_0 + && *(pu+1) == TIXML_UTF_LEAD_1 + && *(pu+2) == TIXML_UTF_LEAD_2 ) { + *bom = true; + p += 3; + } + TIXMLASSERT( p ); + return p; +} + + +void XMLUtil::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) +{ + const unsigned long BYTE_MASK = 0xBF; + const unsigned long BYTE_MARK = 0x80; + const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; + + if (input < 0x80) { + *length = 1; + } + else if ( input < 0x800 ) { + *length = 2; + } + else if ( input < 0x10000 ) { + *length = 3; + } + else if ( input < 0x200000 ) { + *length = 4; + } + else { + *length = 0; // This code won't convert this correctly anyway. + return; + } + + output += *length; + + // Scary scary fall throughs are annotated with carefully designed comments + // to suppress compiler warnings such as -Wimplicit-fallthrough in gcc + switch (*length) { + case 4: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + //fall through + case 3: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + //fall through + case 2: + --output; + *output = (char)((input | BYTE_MARK) & BYTE_MASK); + input >>= 6; + //fall through + case 1: + --output; + *output = (char)(input | FIRST_BYTE_MARK[*length]); + break; + default: + TIXMLASSERT( false ); + } +} + + +const char* XMLUtil::GetCharacterRef( const char* p, char* value, int* length ) +{ + // Presume an entity, and pull it out. + *length = 0; + + if ( *(p+1) == '#' && *(p+2) ) { + unsigned long ucs = 0; + TIXMLASSERT( sizeof( ucs ) >= 4 ); + ptrdiff_t delta = 0; + unsigned mult = 1; + static const char SEMICOLON = ';'; + + if ( *(p+2) == 'x' ) { + // Hexadecimal. + const char* q = p+3; + if ( !(*q) ) { + return nullptr; + } + + q = strchr( q, SEMICOLON ); + + if ( !q ) { + return nullptr; + } + TIXMLASSERT( *q == SEMICOLON ); + + delta = q-p; + --q; + + while ( *q != 'x' ) { + unsigned int digit = 0; + + if ( *q >= '0' && *q <= '9' ) { + digit = *q - '0'; + } + else if ( *q >= 'a' && *q <= 'f' ) { + digit = *q - 'a' + 10; + } + else if ( *q >= 'A' && *q <= 'F' ) { + digit = *q - 'A' + 10; + } + else { + return nullptr; + } + TIXMLASSERT( digit < 16 ); + TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit ); + const unsigned int digitScaled = mult * digit; + TIXMLASSERT( ucs <= ULONG_MAX - digitScaled ); + ucs += digitScaled; + TIXMLASSERT( mult <= UINT_MAX / 16 ); + mult *= 16; + --q; + } + } + else { + // Decimal. + const char* q = p+2; + if ( !(*q) ) { + return nullptr; + } + + q = strchr( q, SEMICOLON ); + + if ( !q ) { + return nullptr; + } + TIXMLASSERT( *q == SEMICOLON ); + + delta = q-p; + --q; + + while ( *q != '#' ) { + if ( *q >= '0' && *q <= '9' ) { + const unsigned int digit = *q - '0'; + TIXMLASSERT( digit < 10 ); + TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit ); + const unsigned int digitScaled = mult * digit; + TIXMLASSERT( ucs <= ULONG_MAX - digitScaled ); + ucs += digitScaled; + } + else { + return nullptr; + } + TIXMLASSERT( mult <= UINT_MAX / 10 ); + mult *= 10; + --q; + } + } + // convert the UCS to UTF-8 + ConvertUTF32ToUTF8( ucs, value, length ); + return p + delta + 1; + } + return p+1; +} + + +void XMLUtil::ToStr( int v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%d", v ); +} + + +void XMLUtil::ToStr( unsigned v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%u", v ); +} + + +void XMLUtil::ToStr( bool v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%s", v ? writeBoolTrue : writeBoolFalse); +} + +/* + ToStr() of a number is a very tricky topic. + https://github.com/leethomason/tinyxml2/issues/106 +*/ +void XMLUtil::ToStr( float v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%.8g", v ); +} + + +void XMLUtil::ToStr( double v, char* buffer, int bufferSize ) +{ + TIXML_SNPRINTF( buffer, bufferSize, "%.17g", v ); +} + + +void XMLUtil::ToStr(int64_t v, char* buffer, int bufferSize) +{ + // horrible syntax trick to make the compiler happy about %lld + TIXML_SNPRINTF(buffer, bufferSize, "%lld", (long long)v); +} + + +bool XMLUtil::ToInt( const char* str, int* value ) +{ + if ( TIXML_SSCANF( str, "%d", value ) == 1 ) { + return true; + } + return false; +} + +bool XMLUtil::ToUnsigned( const char* str, unsigned *value ) +{ + if ( TIXML_SSCANF( str, "%u", value ) == 1 ) { + return true; + } + return false; +} + +bool XMLUtil::ToBool( const char* str, bool* value ) +{ + int ival = 0; + if ( ToInt( str, &ival )) { + *value = (ival==0) ? false : true; + return true; + } + if ( StringEqual( str, "true" ) ) { + *value = true; + return true; + } + else if ( StringEqual( str, "false" ) ) { + *value = false; + return true; + } + return false; +} + + +bool XMLUtil::ToFloat( const char* str, float* value ) +{ + if ( TIXML_SSCANF( str, "%f", value ) == 1 ) { + return true; + } + return false; +} + + +bool XMLUtil::ToDouble( const char* str, double* value ) +{ + if ( TIXML_SSCANF( str, "%lf", value ) == 1 ) { + return true; + } + return false; +} + + +bool XMLUtil::ToInt64(const char* str, int64_t* value) +{ + long long v = 0; // horrible syntax trick to make the compiler happy about %lld + if (TIXML_SSCANF(str, "%lld", &v) == 1) { + *value = (int64_t)v; + return true; + } + return false; +} + + +char* XMLDocument::Identify( char* p, XMLNode** node ) +{ + TIXMLASSERT( node ); + TIXMLASSERT( p ); + char* const start = p; + int const startLine = _parseCurLineNum; + p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum ); + if( !*p ) { + *node = nullptr; + TIXMLASSERT( p ); + return p; + } + + // These strings define the matching patterns: + static const char* xmlHeader = { "<?" }; + static const char* commentHeader = { "<!--" }; + static const char* cdataHeader = { "<![CDATA[" }; + static const char* dtdHeader = { "<!" }; + static const char* elementHeader = { "<" }; // and a header for everything else; check last. + + static const int xmlHeaderLen = 2; + static const int commentHeaderLen = 4; + static const int cdataHeaderLen = 9; + static const int dtdHeaderLen = 2; + static const int elementHeaderLen = 1; + + TIXMLASSERT( sizeof( XMLComment ) == sizeof( XMLUnknown ) ); // use same memory pool + TIXMLASSERT( sizeof( XMLComment ) == sizeof( XMLDeclaration ) ); // use same memory pool + XMLNode* returnNode = nullptr; + if ( XMLUtil::StringEqual( p, xmlHeader, xmlHeaderLen ) ) { + returnNode = CreateUnlinkedNode<XMLDeclaration>( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += xmlHeaderLen; + } + else if ( XMLUtil::StringEqual( p, commentHeader, commentHeaderLen ) ) { + returnNode = CreateUnlinkedNode<XMLComment>( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += commentHeaderLen; + } + else if ( XMLUtil::StringEqual( p, cdataHeader, cdataHeaderLen ) ) { + XMLText* text = CreateUnlinkedNode<XMLText>( _textPool ); + returnNode = text; + returnNode->_parseLineNum = _parseCurLineNum; + p += cdataHeaderLen; + text->SetCData( true ); + } + else if ( XMLUtil::StringEqual( p, dtdHeader, dtdHeaderLen ) ) { + returnNode = CreateUnlinkedNode<XMLUnknown>( _commentPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += dtdHeaderLen; + } + else if ( XMLUtil::StringEqual( p, elementHeader, elementHeaderLen ) ) { + returnNode = CreateUnlinkedNode<XMLElement>( _elementPool ); + returnNode->_parseLineNum = _parseCurLineNum; + p += elementHeaderLen; + } + else { + returnNode = CreateUnlinkedNode<XMLText>( _textPool ); + returnNode->_parseLineNum = _parseCurLineNum; // Report line of first non-whitespace character + p = start; // Back it up, all the text counts. + _parseCurLineNum = startLine; + } + + TIXMLASSERT( returnNode ); + TIXMLASSERT( p ); + *node = returnNode; + return p; +} + + +bool XMLDocument::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + if ( visitor->VisitEnter( *this ) ) { + for ( const XMLNode* node=FirstChild(); node; node=node->NextSibling() ) { + if ( !node->Accept( visitor ) ) { + break; + } + } + } + return visitor->VisitExit( *this ); +} + + +// --------- XMLNode ----------- // + +XMLNode::XMLNode( XMLDocument* doc ) : + _document( doc ), + _parent( nullptr ), + _value(), + _parseLineNum( 0 ), + _firstChild( nullptr ), _lastChild( nullptr ), + _prev( nullptr ), _next( nullptr ), + _userData( nullptr ), + _memPool( nullptr ) +{ +} + + +XMLNode::~XMLNode() +{ + DeleteChildren(); + if ( _parent ) { + _parent->Unlink( this ); + } +} + +const char* XMLNode::Value() const +{ + // Edge case: XMLDocuments don't have a Value. Return null. + if ( this->ToDocument() ) + return nullptr; + return _value.GetStr(); +} + +void XMLNode::SetValue( const char* str, bool staticMem ) +{ + if ( staticMem ) { + _value.SetInternedStr( str ); + } + else { + _value.SetStr( str ); + } +} + +XMLNode* XMLNode::DeepClone(XMLDocument* target) const +{ + XMLNode* clone = this->ShallowClone(target); + if (!clone) return nullptr; + + for (const XMLNode* child = this->FirstChild(); child; child = child->NextSibling()) { + XMLNode* childClone = child->DeepClone(target); + TIXMLASSERT(childClone); + clone->InsertEndChild(childClone); + } + return clone; +} + +void XMLNode::DeleteChildren() +{ + while( _firstChild ) { + TIXMLASSERT( _lastChild ); + DeleteChild( _firstChild ); + } + _firstChild = _lastChild = nullptr; +} + + +void XMLNode::Unlink( XMLNode* child ) +{ + TIXMLASSERT( child ); + TIXMLASSERT( child->_document == _document ); + TIXMLASSERT( child->_parent == this ); + if ( child == _firstChild ) { + _firstChild = _firstChild->_next; + } + if ( child == _lastChild ) { + _lastChild = _lastChild->_prev; + } + + if ( child->_prev ) { + child->_prev->_next = child->_next; + } + if ( child->_next ) { + child->_next->_prev = child->_prev; + } + child->_next = nullptr; + child->_prev = nullptr; + child->_parent = nullptr; +} + + +void XMLNode::DeleteChild( XMLNode* node ) +{ + TIXMLASSERT( node ); + TIXMLASSERT( node->_document == _document ); + TIXMLASSERT( node->_parent == this ); + Unlink( node ); + TIXMLASSERT(node->_prev == nullptr); + TIXMLASSERT(node->_next == nullptr); + TIXMLASSERT(node->_parent == nullptr); + DeleteNode( node ); +} + + +XMLNode* XMLNode::InsertEndChild( XMLNode* addThis ) +{ + TIXMLASSERT( addThis ); + if ( addThis->_document != _document ) { + TIXMLASSERT( false ); + return nullptr; + } + InsertChildPreamble( addThis ); + + if ( _lastChild ) { + TIXMLASSERT( _firstChild ); + TIXMLASSERT( _lastChild->_next == nullptr ); + _lastChild->_next = addThis; + addThis->_prev = _lastChild; + _lastChild = addThis; + + addThis->_next = nullptr; + } + else { + TIXMLASSERT( _firstChild == nullptr ); + _firstChild = _lastChild = addThis; + + addThis->_prev = nullptr; + addThis->_next = nullptr; + } + addThis->_parent = this; + return addThis; +} + + +XMLNode* XMLNode::InsertFirstChild( XMLNode* addThis ) +{ + TIXMLASSERT( addThis ); + if ( addThis->_document != _document ) { + TIXMLASSERT( false ); + return nullptr; + } + InsertChildPreamble( addThis ); + + if ( _firstChild ) { + TIXMLASSERT( _lastChild ); + TIXMLASSERT( _firstChild->_prev == nullptr ); + + _firstChild->_prev = addThis; + addThis->_next = _firstChild; + _firstChild = addThis; + + addThis->_prev = nullptr; + } + else { + TIXMLASSERT( _lastChild == nullptr ); + _firstChild = _lastChild = addThis; + + addThis->_prev = nullptr; + addThis->_next = nullptr; + } + addThis->_parent = this; + return addThis; +} + + +XMLNode* XMLNode::InsertAfterChild( XMLNode* afterThis, XMLNode* addThis ) +{ + TIXMLASSERT( addThis ); + if ( addThis->_document != _document ) { + TIXMLASSERT( false ); + return nullptr; + } + + TIXMLASSERT( afterThis ); + + if ( afterThis->_parent != this ) { + TIXMLASSERT( false ); + return nullptr; + } + if ( afterThis == addThis ) { + // Current state: BeforeThis -> AddThis -> OneAfterAddThis + // Now AddThis must disappear from it's location and then + // reappear between BeforeThis and OneAfterAddThis. + // So just leave it where it is. + return addThis; + } + + if ( afterThis->_next == nullptr ) { + // The last node or the only node. + return InsertEndChild( addThis ); + } + InsertChildPreamble( addThis ); + addThis->_prev = afterThis; + addThis->_next = afterThis->_next; + afterThis->_next->_prev = addThis; + afterThis->_next = addThis; + addThis->_parent = this; + return addThis; +} + + + + +const XMLElement* XMLNode::FirstChildElement( const char* name ) const +{ + for( const XMLNode* node = _firstChild; node; node = node->_next ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return nullptr; +} + + +const XMLElement* XMLNode::LastChildElement( const char* name ) const +{ + for( const XMLNode* node = _lastChild; node; node = node->_prev ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return nullptr; +} + + +const XMLElement* XMLNode::NextSiblingElement( const char* name ) const +{ + for( const XMLNode* node = _next; node; node = node->_next ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return nullptr; +} + + +const XMLElement* XMLNode::PreviousSiblingElement( const char* name ) const +{ + for( const XMLNode* node = _prev; node; node = node->_prev ) { + const XMLElement* element = node->ToElementWithName( name ); + if ( element ) { + return element; + } + } + return nullptr; +} + + +char* XMLNode::ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) +{ + // This is a recursive method, but thinking about it "at the current level" + // it is a pretty simple flat list: + // <foo/> + // <!-- comment --> + // + // With a special case: + // <foo> + // </foo> + // <!-- comment --> + // + // Where the closing element (/foo) *must* be the next thing after the opening + // element, and the names must match. BUT the tricky bit is that the closing + // element will be read by the child. + // + // 'endTag' is the end tag for this node, it is returned by a call to a child. + // 'parentEnd' is the end tag for the parent, which is filled in and returned. + + XMLDocument::DepthTracker tracker(_document); + if (_document->Error()) + return nullptr; + + while( p && *p ) { + XMLNode* node = nullptr; + + p = _document->Identify( p, &node ); + TIXMLASSERT( p ); + if ( node == nullptr ) { + break; + } + + int initialLineNum = node->_parseLineNum; + + StrPair endTag; + p = node->ParseDeep( p, &endTag, curLineNumPtr ); + if ( !p ) { + DeleteNode( node ); + if ( !_document->Error() ) { + _document->SetError( XML_ERROR_PARSING, initialLineNum, nullptr); + } + break; + } + + XMLDeclaration* decl = node->ToDeclaration(); + if ( decl ) { + // Declarations are only allowed at document level + // + // Multiple declarations are allowed but all declarations + // must occur before anything else. + // + // Optimized due to a security test case. If the first node is + // a declaration, and the last node is a declaration, then only + // declarations have so far been addded. + bool wellLocated = false; + + if (ToDocument()) { + if (FirstChild()) { + wellLocated = + FirstChild() && + FirstChild()->ToDeclaration() && + LastChild() && + LastChild()->ToDeclaration(); + } + else { + wellLocated = true; + } + } + if ( !wellLocated ) { + _document->SetError( XML_ERROR_PARSING_DECLARATION, initialLineNum, "XMLDeclaration value=%s", decl->Value()); + DeleteNode( node ); + break; + } + } + + XMLElement* ele = node->ToElement(); + if ( ele ) { + // We read the end tag. Return it to the parent. + if ( ele->ClosingType() == XMLElement::CLOSING ) { + if ( parentEndTag ) { + ele->_value.TransferTo( parentEndTag ); + } + node->_memPool->SetTracked(); // created and then immediately deleted. + DeleteNode( node ); + return p; + } + + // Handle an end tag returned to this level. + // And handle a bunch of annoying errors. + bool mismatch = false; + if ( endTag.Empty() ) { + if ( ele->ClosingType() == XMLElement::OPEN ) { + mismatch = true; + } + } + else { + if ( ele->ClosingType() != XMLElement::OPEN ) { + mismatch = true; + } + else if ( !XMLUtil::StringEqual( endTag.GetStr(), ele->Name() ) ) { + mismatch = true; + } + } + if ( mismatch ) { + _document->SetError( XML_ERROR_MISMATCHED_ELEMENT, initialLineNum, "XMLElement name=%s", ele->Name()); + DeleteNode( node ); + break; + } + } + InsertEndChild( node ); + } + return nullptr; +} + +/*static*/ void XMLNode::DeleteNode( XMLNode* node ) +{ + if ( node == nullptr ) { + return; + } + TIXMLASSERT(node->_document); + if (!node->ToDocument()) { + node->_document->MarkInUse(node); + } + + MemPool* pool = node->_memPool; + node->~XMLNode(); + pool->Free( node ); +} + +void XMLNode::InsertChildPreamble( XMLNode* insertThis ) const +{ + TIXMLASSERT( insertThis ); + TIXMLASSERT( insertThis->_document == _document ); + + if (insertThis->_parent) { + insertThis->_parent->Unlink( insertThis ); + } + else { + insertThis->_document->MarkInUse(insertThis); + insertThis->_memPool->SetTracked(); + } +} + +const XMLElement* XMLNode::ToElementWithName( const char* name ) const +{ + const XMLElement* element = this->ToElement(); + if ( element == nullptr ) { + return nullptr; + } + if ( name == nullptr ) { + return element; + } + if ( XMLUtil::StringEqual( element->Name(), name ) ) { + return element; + } + return nullptr; +} + +// --------- XMLText ---------- // +char* XMLText::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + if ( this->CData() ) { + p = _value.ParseText( p, "]]>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); + if ( !p ) { + _document->SetError( XML_ERROR_PARSING_CDATA, _parseLineNum, nullptr ); + } + return p; + } + else { + int flags = _document->ProcessEntities() ? StrPair::TEXT_ELEMENT : StrPair::TEXT_ELEMENT_LEAVE_ENTITIES; + if ( _document->WhitespaceMode() == COLLAPSE_WHITESPACE ) { + flags |= StrPair::NEEDS_WHITESPACE_COLLAPSING; + } + + p = _value.ParseText( p, "<", flags, curLineNumPtr ); + if ( p && *p ) { + return p-1; + } + if ( !p ) { + _document->SetError( XML_ERROR_PARSING_TEXT, _parseLineNum, nullptr ); + } + } + return nullptr; +} + + +XMLNode* XMLText::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLText* text = doc->NewText( Value() ); // fixme: this will always allocate memory. Intern? + text->SetCData( this->CData() ); + return text; +} + + +bool XMLText::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLText* text = compare->ToText(); + return ( text && XMLUtil::StringEqual( text->Value(), Value() ) ); +} + + +bool XMLText::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + + +// --------- XMLComment ---------- // + +XMLComment::XMLComment( XMLDocument* doc ) : XMLNode( doc ) +{ +} + + +XMLComment::~XMLComment() += default; + + +char* XMLComment::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + // Comment parses as text. + p = _value.ParseText( p, "-->", StrPair::COMMENT, curLineNumPtr ); + if ( p == nullptr ) { + _document->SetError( XML_ERROR_PARSING_COMMENT, _parseLineNum, nullptr ); + } + return p; +} + + +XMLNode* XMLComment::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLComment* comment = doc->NewComment( Value() ); // fixme: this will always allocate memory. Intern? + return comment; +} + + +bool XMLComment::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLComment* comment = compare->ToComment(); + return ( comment && XMLUtil::StringEqual( comment->Value(), Value() )); +} + + +bool XMLComment::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + + +// --------- XMLDeclaration ---------- // + +XMLDeclaration::XMLDeclaration( XMLDocument* doc ) : XMLNode( doc ) +{ +} + + +XMLDeclaration::~XMLDeclaration() +{ + //printf( "~XMLDeclaration\n" ); +} + + +char* XMLDeclaration::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + // Declaration parses as text. + p = _value.ParseText( p, "?>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); + if ( p == nullptr ) { + _document->SetError( XML_ERROR_PARSING_DECLARATION, _parseLineNum, nullptr ); + } + return p; +} + + +XMLNode* XMLDeclaration::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLDeclaration* dec = doc->NewDeclaration( Value() ); // fixme: this will always allocate memory. Intern? + return dec; +} + + +bool XMLDeclaration::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLDeclaration* declaration = compare->ToDeclaration(); + return ( declaration && XMLUtil::StringEqual( declaration->Value(), Value() )); +} + + + +bool XMLDeclaration::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + +// --------- XMLUnknown ---------- // + +XMLUnknown::XMLUnknown( XMLDocument* doc ) : XMLNode( doc ) +{ +} + + +XMLUnknown::~XMLUnknown() += default; + + +char* XMLUnknown::ParseDeep( char* p, StrPair*, int* curLineNumPtr ) +{ + // Unknown parses as text. + p = _value.ParseText( p, ">", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr ); + if ( !p ) { + _document->SetError( XML_ERROR_PARSING_UNKNOWN, _parseLineNum, nullptr ); + } + return p; +} + + +XMLNode* XMLUnknown::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLUnknown* text = doc->NewUnknown( Value() ); // fixme: this will always allocate memory. Intern? + return text; +} + + +bool XMLUnknown::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLUnknown* unknown = compare->ToUnknown(); + return ( unknown && XMLUtil::StringEqual( unknown->Value(), Value() )); +} + + +bool XMLUnknown::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + return visitor->Visit( *this ); +} + +// --------- XMLAttribute ---------- // + +const char* XMLAttribute::Name() const +{ + return _name.GetStr(); +} + +const char* XMLAttribute::Value() const +{ + return _value.GetStr(); +} + +char* XMLAttribute::ParseDeep( char* p, bool processEntities, int* curLineNumPtr ) +{ + // Parse using the name rules: bug fix, was using ParseText before + p = _name.ParseName( p ); + if ( !p || !*p ) { + return nullptr; + } + + // Skip white space before = + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + if ( *p != '=' ) { + return nullptr; + } + + ++p; // move up to opening quote + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + if ( *p != '\"' && *p != '\'' ) { + return nullptr; + } + + char endTag[2] = { *p, 0 }; + ++p; // move past opening quote + + p = _value.ParseText( p, endTag, processEntities ? StrPair::ATTRIBUTE_VALUE : StrPair::ATTRIBUTE_VALUE_LEAVE_ENTITIES, curLineNumPtr ); + return p; +} + + +void XMLAttribute::SetName( const char* n ) +{ + _name.SetStr( n ); +} + + +XMLError XMLAttribute::QueryIntValue( int* value ) const +{ + if ( XMLUtil::ToInt( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryUnsignedValue( unsigned int* value ) const +{ + if ( XMLUtil::ToUnsigned( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryInt64Value(int64_t* value) const +{ + if (XMLUtil::ToInt64(Value(), value)) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryBoolValue( bool* value ) const +{ + if ( XMLUtil::ToBool( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryFloatValue( float* value ) const +{ + if ( XMLUtil::ToFloat( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +XMLError XMLAttribute::QueryDoubleValue( double* value ) const +{ + if ( XMLUtil::ToDouble( Value(), value )) { + return XML_SUCCESS; + } + return XML_WRONG_ATTRIBUTE_TYPE; +} + + +void XMLAttribute::SetAttribute( const char* v ) +{ + _value.SetStr( v ); +} + + +void XMLAttribute::SetAttribute( int v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + + +void XMLAttribute::SetAttribute( unsigned v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + + +void XMLAttribute::SetAttribute(int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + _value.SetStr(buf); +} + + + +void XMLAttribute::SetAttribute( bool v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + +void XMLAttribute::SetAttribute( double v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + +void XMLAttribute::SetAttribute( float v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + _value.SetStr( buf ); +} + + +// --------- XMLElement ---------- // +XMLElement::XMLElement( XMLDocument* doc ) : XMLNode( doc ), + _closingType( OPEN ), + _rootAttribute( nullptr ) +{ +} + + +XMLElement::~XMLElement() +{ + while( _rootAttribute ) { + XMLAttribute* next = _rootAttribute->_next; + DeleteAttribute( _rootAttribute ); + _rootAttribute = next; + } +} + + +const XMLAttribute* XMLElement::FindAttribute( const char* name ) const +{ + for( XMLAttribute* a = _rootAttribute; a; a = a->_next ) { + if ( XMLUtil::StringEqual( a->Name(), name ) ) { + return a; + } + } + return nullptr; +} + + +const char* XMLElement::Attribute( const char* name, const char* value ) const +{ + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return nullptr; + } + if ( !value || XMLUtil::StringEqual( a->Value(), value )) { + return a->Value(); + } + return nullptr; +} + +int XMLElement::IntAttribute(const char* name, int defaultValue) const +{ + int i = defaultValue; + QueryIntAttribute(name, &i); + return i; +} + +unsigned XMLElement::UnsignedAttribute(const char* name, unsigned defaultValue) const +{ + unsigned i = defaultValue; + QueryUnsignedAttribute(name, &i); + return i; +} + +int64_t XMLElement::Int64Attribute(const char* name, int64_t defaultValue) const +{ + int64_t i = defaultValue; + QueryInt64Attribute(name, &i); + return i; +} + +bool XMLElement::BoolAttribute(const char* name, bool defaultValue) const +{ + bool b = defaultValue; + QueryBoolAttribute(name, &b); + return b; +} + +double XMLElement::DoubleAttribute(const char* name, double defaultValue) const +{ + double d = defaultValue; + QueryDoubleAttribute(name, &d); + return d; +} + +float XMLElement::FloatAttribute(const char* name, float defaultValue) const +{ + float f = defaultValue; + QueryFloatAttribute(name, &f); + return f; +} + +const char* XMLElement::GetText() const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + return FirstChild()->Value(); + } + return nullptr; +} + + +void XMLElement::SetText( const char* inText ) +{ + if ( FirstChild() && FirstChild()->ToText() ) + FirstChild()->SetValue( inText ); + else { + XMLText* theText = GetDocument()->NewText( inText ); + InsertFirstChild( theText ); + } +} + + +void XMLElement::SetText( int v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText( unsigned v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText(int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + SetText(buf); +} + + +void XMLElement::SetText( bool v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText( float v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +void XMLElement::SetText( double v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + SetText( buf ); +} + + +XMLError XMLElement::QueryIntText( int* ival ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToInt( t, ival ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryUnsignedText( unsigned* uval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToUnsigned( t, uval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryInt64Text(int64_t* ival) const +{ + if (FirstChild() && FirstChild()->ToText()) { + const char* t = FirstChild()->Value(); + if (XMLUtil::ToInt64(t, ival)) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryBoolText( bool* bval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToBool( t, bval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryDoubleText( double* dval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToDouble( t, dval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + + +XMLError XMLElement::QueryFloatText( float* fval ) const +{ + if ( FirstChild() && FirstChild()->ToText() ) { + const char* t = FirstChild()->Value(); + if ( XMLUtil::ToFloat( t, fval ) ) { + return XML_SUCCESS; + } + return XML_CAN_NOT_CONVERT_TEXT; + } + return XML_NO_TEXT_NODE; +} + +int XMLElement::IntText(int defaultValue) const +{ + int i = defaultValue; + QueryIntText(&i); + return i; +} + +unsigned XMLElement::UnsignedText(unsigned defaultValue) const +{ + unsigned i = defaultValue; + QueryUnsignedText(&i); + return i; +} + +int64_t XMLElement::Int64Text(int64_t defaultValue) const +{ + int64_t i = defaultValue; + QueryInt64Text(&i); + return i; +} + +bool XMLElement::BoolText(bool defaultValue) const +{ + bool b = defaultValue; + QueryBoolText(&b); + return b; +} + +double XMLElement::DoubleText(double defaultValue) const +{ + double d = defaultValue; + QueryDoubleText(&d); + return d; +} + +float XMLElement::FloatText(float defaultValue) const +{ + float f = defaultValue; + QueryFloatText(&f); + return f; +} + + +XMLAttribute* XMLElement::FindOrCreateAttribute( const char* name ) +{ + XMLAttribute* last = nullptr; + XMLAttribute* attrib = nullptr; + for( attrib = _rootAttribute; + attrib; + last = attrib, attrib = attrib->_next ) { + if ( XMLUtil::StringEqual( attrib->Name(), name ) ) { + break; + } + } + if ( !attrib ) { + attrib = CreateAttribute(); + TIXMLASSERT( attrib ); + if ( last ) { + TIXMLASSERT( last->_next == nullptr ); + last->_next = attrib; + } + else { + TIXMLASSERT( _rootAttribute == nullptr ); + _rootAttribute = attrib; + } + attrib->SetName( name ); + } + return attrib; +} + + +void XMLElement::DeleteAttribute( const char* name ) +{ + XMLAttribute* prev = nullptr; + for( XMLAttribute* a=_rootAttribute; a; a=a->_next ) { + if ( XMLUtil::StringEqual( name, a->Name() ) ) { + if ( prev ) { + prev->_next = a->_next; + } + else { + _rootAttribute = a->_next; + } + DeleteAttribute( a ); + break; + } + prev = a; + } +} + + +char* XMLElement::ParseAttributes( char* p, int* curLineNumPtr ) +{ + XMLAttribute* prevAttribute = nullptr; + + // Read the attributes. + while( p ) { + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + if ( !(*p) ) { + _document->SetError( XML_ERROR_PARSING_ELEMENT, _parseLineNum, "XMLElement name=%s", Name() ); + return nullptr; + } + + // attribute. + if (XMLUtil::IsNameStartChar( *p ) ) { + XMLAttribute* attrib = CreateAttribute(); + TIXMLASSERT( attrib ); + attrib->_parseLineNum = _document->_parseCurLineNum; + + int attrLineNum = attrib->_parseLineNum; + + p = attrib->ParseDeep( p, _document->ProcessEntities(), curLineNumPtr ); + if ( !p || Attribute( attrib->Name() ) ) { + DeleteAttribute( attrib ); + _document->SetError( XML_ERROR_PARSING_ATTRIBUTE, attrLineNum, "XMLElement name=%s", Name() ); + return nullptr; + } + // There is a minor bug here: if the attribute in the source xml + // document is duplicated, it will not be detected and the + // attribute will be doubly added. However, tracking the 'prevAttribute' + // avoids re-scanning the attribute list. Preferring performance for + // now, may reconsider in the future. + if ( prevAttribute ) { + TIXMLASSERT( prevAttribute->_next == nullptr ); + prevAttribute->_next = attrib; + } + else { + TIXMLASSERT( _rootAttribute == nullptr ); + _rootAttribute = attrib; + } + prevAttribute = attrib; + } + // end of the tag + else if ( *p == '>' ) { + ++p; + break; + } + // end of the tag + else if ( *p == '/' && *(p+1) == '>' ) { + _closingType = CLOSED; + return p+2; // done; sealed element. + } + else { + _document->SetError( XML_ERROR_PARSING_ELEMENT, _parseLineNum, nullptr ); + return nullptr; + } + } + return p; +} + +void XMLElement::DeleteAttribute( XMLAttribute* attribute ) +{ + if ( attribute == nullptr ) { + return; + } + MemPool* pool = attribute->_memPool; + attribute->~XMLAttribute(); + pool->Free( attribute ); +} + +XMLAttribute* XMLElement::CreateAttribute() +{ + TIXMLASSERT( sizeof( XMLAttribute ) == _document->_attributePool.ItemSize() ); + XMLAttribute* attrib = new (_document->_attributePool.Alloc() ) XMLAttribute(); + TIXMLASSERT( attrib ); + attrib->_memPool = &_document->_attributePool; + attrib->_memPool->SetTracked(); + return attrib; +} + +// +// <ele></ele> +// <ele>foo<b>bar</b></ele> +// +char* XMLElement::ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) +{ + // Read the element name. + p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr ); + + // The closing element is the </element> form. It is + // parsed just like a regular element then deleted from + // the DOM. + if ( *p == '/' ) { + _closingType = CLOSING; + ++p; + } + + p = _value.ParseName( p ); + if ( _value.Empty() ) { + return nullptr; + } + + p = ParseAttributes( p, curLineNumPtr ); + if ( !p || !*p || _closingType != OPEN ) { + return p; + } + + p = XMLNode::ParseDeep( p, parentEndTag, curLineNumPtr ); + return p; +} + + + +XMLNode* XMLElement::ShallowClone( XMLDocument* doc ) const +{ + if ( !doc ) { + doc = _document; + } + XMLElement* element = doc->NewElement( Value() ); // fixme: this will always allocate memory. Intern? + for( const XMLAttribute* a=FirstAttribute(); a; a=a->Next() ) { + element->SetAttribute( a->Name(), a->Value() ); // fixme: this will always allocate memory. Intern? + } + return element; +} + + +bool XMLElement::ShallowEqual( const XMLNode* compare ) const +{ + TIXMLASSERT( compare ); + const XMLElement* other = compare->ToElement(); + if ( other && XMLUtil::StringEqual( other->Name(), Name() )) { + + const XMLAttribute* a=FirstAttribute(); + const XMLAttribute* b=other->FirstAttribute(); + + while ( a && b ) { + if ( !XMLUtil::StringEqual( a->Value(), b->Value() ) ) { + return false; + } + a = a->Next(); + b = b->Next(); + } + if ( a || b ) { + // different count + return false; + } + return true; + } + return false; +} + + +bool XMLElement::Accept( XMLVisitor* visitor ) const +{ + TIXMLASSERT( visitor ); + if ( visitor->VisitEnter( *this, _rootAttribute ) ) { + for ( const XMLNode* node=FirstChild(); node; node=node->NextSibling() ) { + if ( !node->Accept( visitor ) ) { + break; + } + } + } + return visitor->VisitExit( *this ); +} + + +// --------- XMLDocument ----------- // + +// Warning: List must match 'enum XMLError' +const char* XMLDocument::_errorNames[XML_ERROR_COUNT] = { + "XML_SUCCESS", + "XML_NO_ATTRIBUTE", + "XML_WRONG_ATTRIBUTE_TYPE", + "XML_ERROR_FILE_NOT_FOUND", + "XML_ERROR_FILE_COULD_NOT_BE_OPENED", + "XML_ERROR_FILE_READ_ERROR", + "XML_ERROR_PARSING_ELEMENT", + "XML_ERROR_PARSING_ATTRIBUTE", + "XML_ERROR_PARSING_TEXT", + "XML_ERROR_PARSING_CDATA", + "XML_ERROR_PARSING_COMMENT", + "XML_ERROR_PARSING_DECLARATION", + "XML_ERROR_PARSING_UNKNOWN", + "XML_ERROR_EMPTY_DOCUMENT", + "XML_ERROR_MISMATCHED_ELEMENT", + "XML_ERROR_PARSING", + "XML_CAN_NOT_CONVERT_TEXT", + "XML_NO_TEXT_NODE", + "XML_ELEMENT_DEPTH_EXCEEDED" +}; + + +XMLDocument::XMLDocument( bool processEntities, Whitespace whitespaceMode ) : + XMLNode( nullptr ), + _writeBOM( false ), + _processEntities( processEntities ), + _errorID(XML_SUCCESS), + _whitespaceMode( whitespaceMode ), + _errorStr(), + _errorLineNum( 0 ), + _charBuffer( nullptr ), + _parseCurLineNum( 0 ), + _parsingDepth(0), + _unlinked(), + _elementPool(), + _attributePool(), + _textPool(), + _commentPool() +{ + // avoid VC++ C4355 warning about 'this' in initializer list (C4355 is off by default in VS2012+) + _document = this; +} + + +XMLDocument::~XMLDocument() +{ + Clear(); +} + + +void XMLDocument::MarkInUse(XMLNode* node) +{ + TIXMLASSERT(node); + TIXMLASSERT(node->_parent == nullptr); + + for (int i = 0; i < _unlinked.Size(); ++i) { + if (node == _unlinked[i]) { + _unlinked.SwapRemove(i); + break; + } + } +} + +void XMLDocument::Clear() +{ + DeleteChildren(); + while( _unlinked.Size()) { + DeleteNode(_unlinked[0]); // Will remove from _unlinked as part of delete. + } + +#ifdef TINYXML2_DEBUG + const bool hadError = Error(); +#endif + ClearError(); + + delete [] _charBuffer; + _charBuffer = nullptr; + _parsingDepth = 0; + +#if 0 + _textPool.Trace( "text" ); + _elementPool.Trace( "element" ); + _commentPool.Trace( "comment" ); + _attributePool.Trace( "attribute" ); +#endif + +#ifdef TINYXML2_DEBUG + if ( !hadError ) { + TIXMLASSERT( _elementPool.CurrentAllocs() == _elementPool.Untracked() ); + TIXMLASSERT( _attributePool.CurrentAllocs() == _attributePool.Untracked() ); + TIXMLASSERT( _textPool.CurrentAllocs() == _textPool.Untracked() ); + TIXMLASSERT( _commentPool.CurrentAllocs() == _commentPool.Untracked() ); + } +#endif +} + + +void XMLDocument::DeepCopy(XMLDocument* target) const +{ + TIXMLASSERT(target); + if (target == this) { + return; // technically success - a no-op. + } + + target->Clear(); + for (const XMLNode* node = this->FirstChild(); node; node = node->NextSibling()) { + target->InsertEndChild(node->DeepClone(target)); + } +} + +XMLElement* XMLDocument::NewElement( const char* name ) +{ + XMLElement* ele = CreateUnlinkedNode<XMLElement>( _elementPool ); + ele->SetName( name ); + return ele; +} + + +XMLComment* XMLDocument::NewComment( const char* str ) +{ + XMLComment* comment = CreateUnlinkedNode<XMLComment>( _commentPool ); + comment->SetValue( str ); + return comment; +} + + +XMLText* XMLDocument::NewText( const char* str ) +{ + XMLText* text = CreateUnlinkedNode<XMLText>( _textPool ); + text->SetValue( str ); + return text; +} + + +XMLDeclaration* XMLDocument::NewDeclaration( const char* str ) +{ + XMLDeclaration* dec = CreateUnlinkedNode<XMLDeclaration>( _commentPool ); + dec->SetValue( str ? str : "xml version=\"1.0\" encoding=\"UTF-8\"" ); + return dec; +} + + +XMLUnknown* XMLDocument::NewUnknown( const char* str ) +{ + XMLUnknown* unk = CreateUnlinkedNode<XMLUnknown>( _commentPool ); + unk->SetValue( str ); + return unk; +} + +static FILE* callfopen( const char* filepath, const char* mode ) +{ + TIXMLASSERT( filepath ); + TIXMLASSERT( mode ); +#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) && (!defined WINCE) + FILE* fp = 0; + errno_t err = fopen_s( &fp, filepath, mode ); + if ( err ) { + return 0; + } +#else + FILE* fp = fopen( filepath, mode ); +#endif + return fp; +} + +void XMLDocument::DeleteNode( XMLNode* node ) { + TIXMLASSERT( node ); + TIXMLASSERT(node->_document == this ); + if (node->_parent) { + node->_parent->DeleteChild( node ); + } + else { + // Isn't in the tree. + // Use the parent delete. + // Also, we need to mark it tracked: we 'know' + // it was never used. + node->_memPool->SetTracked(); + // Call the static XMLNode version: + XMLNode::DeleteNode(node); + } +} + + +XMLError XMLDocument::LoadFile( const char* filename ) +{ + if ( !filename ) { + TIXMLASSERT( false ); + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=<null>" ); + return _errorID; + } + + Clear(); + FILE* fp = callfopen( filename, "rb" ); + if ( !fp ) { + SetError( XML_ERROR_FILE_NOT_FOUND, 0, "filename=%s", filename ); + return _errorID; + } + LoadFile( fp ); + fclose( fp ); + return _errorID; +} + +// This is likely overengineered template art to have a check that unsigned long value incremented +// by one still fits into size_t. If size_t type is larger than unsigned long type +// (x86_64-w64-mingw32 target) then the check is redundant and gcc and clang emit +// -Wtype-limits warning. This piece makes the compiler select code with a check when a check +// is useful and code with no check when a check is redundant depending on how size_t and unsigned long +// types sizes relate to each other. +template +<bool = (sizeof(unsigned long) >= sizeof(size_t))> +struct LongFitsIntoSizeTMinusOne { + static bool Fits( unsigned long value ) + { + return value < (size_t)-1; + } +}; + +template <> +struct LongFitsIntoSizeTMinusOne<false> { + static bool Fits( unsigned long ) + { + return true; + } +}; + +XMLError XMLDocument::LoadFile( FILE* fp ) +{ + Clear(); + + fseek( fp, 0, SEEK_SET ); + if ( fgetc( fp ) == EOF && ferror( fp ) != 0 ) { + SetError( XML_ERROR_FILE_READ_ERROR, 0, nullptr ); + return _errorID; + } + + fseek( fp, 0, SEEK_END ); + const long filelength = ftell( fp ); + fseek( fp, 0, SEEK_SET ); + if ( filelength == -1L ) { + SetError( XML_ERROR_FILE_READ_ERROR, 0, nullptr ); + return _errorID; + } + TIXMLASSERT( filelength >= 0 ); + + if ( !LongFitsIntoSizeTMinusOne<>::Fits( filelength ) ) { + // Cannot handle files which won't fit in buffer together with null terminator + SetError( XML_ERROR_FILE_READ_ERROR, 0, nullptr ); + return _errorID; + } + + if ( filelength == 0 ) { + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, nullptr ); + return _errorID; + } + + const size_t size = filelength; + TIXMLASSERT( _charBuffer == nullptr ); + _charBuffer = new char[size+1]; + size_t read = fread( _charBuffer, 1, size, fp ); + if ( read != size ) { + SetError( XML_ERROR_FILE_READ_ERROR, 0, nullptr ); + return _errorID; + } + + _charBuffer[size] = 0; + + Parse(); + return _errorID; +} + + +XMLError XMLDocument::SaveFile( const char* filename, bool compact ) +{ + if ( !filename ) { + TIXMLASSERT( false ); + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=<null>" ); + return _errorID; + } + + FILE* fp = callfopen( filename, "w" ); + if ( !fp ) { + SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=%s", filename ); + return _errorID; + } + SaveFile(fp, compact); + fclose( fp ); + return _errorID; +} + + +XMLError XMLDocument::SaveFile( FILE* fp, bool compact ) +{ + // Clear any error from the last save, otherwise it will get reported + // for *this* call. + ClearError(); + XMLPrinter stream( fp, compact ); + Print( &stream ); + return _errorID; +} + + +XMLError XMLDocument::Parse( const char* p, size_t len ) +{ + Clear(); + + if ( len == 0 || !p || !*p ) { + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, nullptr ); + return _errorID; + } + if ( len == (size_t)(-1) ) { + len = strlen( p ); + } + TIXMLASSERT( _charBuffer == nullptr ); + _charBuffer = new char[ len+1 ]; + memcpy( _charBuffer, p, len ); + _charBuffer[len] = 0; + + Parse(); + if ( Error() ) { + // clean up now essentially dangling memory. + // and the parse fail can put objects in the + // pools that are dead and inaccessible. + DeleteChildren(); + _elementPool.Clear(); + _attributePool.Clear(); + _textPool.Clear(); + _commentPool.Clear(); + } + return _errorID; +} + + +void XMLDocument::Print( XMLPrinter* streamer ) const +{ + if ( streamer ) { + Accept( streamer ); + } + else { + XMLPrinter stdoutStreamer( stdout ); + Accept( &stdoutStreamer ); + } +} + + +void XMLDocument::SetError( XMLError error, int lineNum, const char* format, ... ) +{ + TIXMLASSERT( error >= 0 && error < XML_ERROR_COUNT ); + _errorID = error; + _errorLineNum = lineNum; + _errorStr.Reset(); + + size_t BUFFER_SIZE = 1000; + char* buffer = new char[BUFFER_SIZE]; + + TIXMLASSERT(sizeof(error) <= sizeof(int)); + TIXML_SNPRINTF(buffer, BUFFER_SIZE, "Error=%s ErrorID=%d (0x%x) Line number=%d", ErrorIDToName(error), int(error), int(error), lineNum); + + if (format) { + size_t len = strlen(buffer); + TIXML_SNPRINTF(buffer + len, BUFFER_SIZE - len, ": "); + len = strlen(buffer); + + va_list va; + va_start(va, format); + TIXML_VSNPRINTF(buffer + len, BUFFER_SIZE - len, format, va); + va_end(va); + } + _errorStr.SetStr(buffer); + delete[] buffer; +} + + +/*static*/ const char* XMLDocument::ErrorIDToName(XMLError errorID) +{ + TIXMLASSERT( errorID >= 0 && errorID < XML_ERROR_COUNT ); + const char* errorName = _errorNames[errorID]; + TIXMLASSERT( errorName && errorName[0] ); + return errorName; +} + +const char* XMLDocument::ErrorStr() const +{ + return _errorStr.Empty() ? "" : _errorStr.GetStr(); +} + + +void XMLDocument::PrintError() const +{ + printf("%s\n", ErrorStr()); +} + +const char* XMLDocument::ErrorName() const +{ + return ErrorIDToName(_errorID); +} + +void XMLDocument::Parse() +{ + TIXMLASSERT( NoChildren() ); // Clear() must have been called previously + TIXMLASSERT( _charBuffer ); + _parseCurLineNum = 1; + _parseLineNum = 1; + char* p = _charBuffer; + p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum ); + p = const_cast<char*>( XMLUtil::ReadBOM( p, &_writeBOM ) ); + if ( !*p ) { + SetError( XML_ERROR_EMPTY_DOCUMENT, 0, nullptr ); + return; + } + ParseDeep(p, nullptr, &_parseCurLineNum ); +} + +void XMLDocument::PushDepth() +{ + _parsingDepth++; + if (_parsingDepth == TINYXML2_MAX_ELEMENT_DEPTH) { + SetError(XML_ELEMENT_DEPTH_EXCEEDED, _parseCurLineNum, "Element nesting is too deep." ); + } +} + +void XMLDocument::PopDepth() +{ + TIXMLASSERT(_parsingDepth > 0); + --_parsingDepth; +} + +XMLPrinter::XMLPrinter( FILE* file, bool compact, int depth ) : + _elementJustOpened( false ), + _stack(), + _firstElement( true ), + _fp( file ), + _depth( depth ), + _textDepth( -1 ), + _processEntities( true ), + _compactMode( compact ), + _buffer() +{ + for( int i=0; i<ENTITY_RANGE; ++i ) { + _entityFlag[i] = false; + _restrictedEntityFlag[i] = false; + } + for(auto entitie : entities) { + const char entityValue = entitie.value; + const unsigned char flagIndex = (unsigned char)entityValue; + TIXMLASSERT( flagIndex < ENTITY_RANGE ); + _entityFlag[flagIndex] = true; + } + _restrictedEntityFlag[(unsigned char)'&'] = true; + _restrictedEntityFlag[(unsigned char)'<'] = true; + _restrictedEntityFlag[(unsigned char)'>'] = true; // not required, but consistency is nice + _buffer.Push( 0 ); +} + + +void XMLPrinter::Print( const char* format, ... ) +{ + va_list va; + va_start( va, format ); + + if ( _fp ) { + vfprintf( _fp, format, va ); + } + else { + const int len = TIXML_VSCPRINTF( format, va ); + // Close out and re-start the va-args + va_end( va ); + TIXMLASSERT( len >= 0 ); + va_start( va, format ); + TIXMLASSERT( _buffer.Size() > 0 && _buffer[_buffer.Size() - 1] == 0 ); + char* p = _buffer.PushArr( len ) - 1; // back up over the null terminator. + TIXML_VSNPRINTF( p, len+1, format, va ); + } + va_end( va ); +} + + +void XMLPrinter::Write( const char* data, size_t size ) +{ + if ( _fp ) { + fwrite ( data , sizeof(char), size, _fp); + } + else { + char* p = _buffer.PushArr( static_cast<int>(size) ) - 1; // back up over the null terminator. + memcpy( p, data, size ); + p[size] = 0; + } +} + + +void XMLPrinter::Putc( char ch ) +{ + if ( _fp ) { + fputc ( ch, _fp); + } + else { + char* p = _buffer.PushArr( sizeof(char) ) - 1; // back up over the null terminator. + p[0] = ch; + p[1] = 0; + } +} + + +void XMLPrinter::PrintSpace( int depth ) +{ + for( int i=0; i<depth; ++i ) { + Write( " " ); + } +} + + +void XMLPrinter::PrintString( const char* p, bool restricted ) +{ + // Look for runs of bytes between entities to print. + const char* q = p; + + if ( _processEntities ) { + const bool* flag = restricted ? _restrictedEntityFlag : _entityFlag; + while ( *q ) { + TIXMLASSERT( p <= q ); + // Remember, char is sometimes signed. (How many times has that bitten me?) + if ( *q > 0 && *q < ENTITY_RANGE ) { + // Check for entities. If one is found, flush + // the stream up until the entity, write the + // entity, and keep looking. + if ( flag[(unsigned char)(*q)] ) { + while ( p < q ) { + const size_t delta = q - p; + const int toPrint = ( INT_MAX < delta ) ? INT_MAX : (int)delta; + Write( p, toPrint ); + p += toPrint; + } + bool entityPatternPrinted = false; + for(auto entitie : entities) { + if ( entitie.value == *q ) { + Putc( '&' ); + Write( entitie.pattern, entitie.length ); + Putc( ';' ); + entityPatternPrinted = true; + break; + } + } + if ( !entityPatternPrinted ) { + // TIXMLASSERT( entityPatternPrinted ) causes gcc -Wunused-but-set-variable in release + TIXMLASSERT( false ); + } + ++p; + } + } + ++q; + TIXMLASSERT( p <= q ); + } + // Flush the remaining string. This will be the entire + // string if an entity wasn't found. + if ( p < q ) { + const size_t delta = q - p; + const int toPrint = ( INT_MAX < delta ) ? INT_MAX : (int)delta; + Write( p, toPrint ); + } + } + else { + Write( p ); + } +} + + +void XMLPrinter::PushHeader( bool writeBOM, bool writeDec ) +{ + if ( writeBOM ) { + static const unsigned char bom[] = { TIXML_UTF_LEAD_0, TIXML_UTF_LEAD_1, TIXML_UTF_LEAD_2, 0 }; + Write( reinterpret_cast< const char* >( bom ) ); + } + if ( writeDec ) { + PushDeclaration( "xml version=\"1.0\"" ); + } +} + + +void XMLPrinter::OpenElement( const char* name, bool compactMode ) +{ + SealElementIfJustOpened(); + _stack.Push( name ); + + if ( _textDepth < 0 && !_firstElement && !compactMode ) { + Putc( '\n' ); + } + if ( !compactMode ) { + PrintSpace( _depth ); + } + + Write ( "<" ); + Write ( name ); + + _elementJustOpened = true; + _firstElement = false; + ++_depth; +} + + +void XMLPrinter::PushAttribute( const char* name, const char* value ) +{ + TIXMLASSERT( _elementJustOpened ); + Putc ( ' ' ); + Write( name ); + Write( "=\"" ); + PrintString( value, false ); + Putc ( '\"' ); +} + + +void XMLPrinter::PushAttribute( const char* name, int v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::PushAttribute( const char* name, unsigned v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::PushAttribute(const char* name, int64_t v) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr(v, buf, BUF_SIZE); + PushAttribute(name, buf); +} + + +void XMLPrinter::PushAttribute( const char* name, bool v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::PushAttribute( const char* name, double v ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( v, buf, BUF_SIZE ); + PushAttribute( name, buf ); +} + + +void XMLPrinter::CloseElement( bool compactMode ) +{ + --_depth; + const char* name = _stack.Pop(); + + if ( _elementJustOpened ) { + Write( "/>" ); + } + else { + if ( _textDepth < 0 && !compactMode) { + Putc( '\n' ); + PrintSpace( _depth ); + } + Write ( "</" ); + Write ( name ); + Write ( ">" ); + } + + if ( _textDepth == _depth ) { + _textDepth = -1; + } + if ( _depth == 0 && !compactMode) { + Putc( '\n' ); + } + _elementJustOpened = false; +} + + +void XMLPrinter::SealElementIfJustOpened() +{ + if ( !_elementJustOpened ) { + return; + } + _elementJustOpened = false; + Putc( '>' ); +} + + +void XMLPrinter::PushText( const char* text, bool cdata ) +{ + _textDepth = _depth-1; + + SealElementIfJustOpened(); + if ( cdata ) { + Write( "<![CDATA[" ); + Write( text ); + Write( "]]>" ); + } + else { + PrintString( text, true ); + } +} + +void XMLPrinter::PushText( int64_t value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + +void XMLPrinter::PushText( int value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( unsigned value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( bool value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( float value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushText( double value ) +{ + char buf[BUF_SIZE]; + XMLUtil::ToStr( value, buf, BUF_SIZE ); + PushText( buf, false ); +} + + +void XMLPrinter::PushComment( const char* comment ) +{ + SealElementIfJustOpened(); + if ( _textDepth < 0 && !_firstElement && !_compactMode) { + Putc( '\n' ); + PrintSpace( _depth ); + } + _firstElement = false; + + Write( "<!--" ); + Write( comment ); + Write( "-->" ); +} + + +void XMLPrinter::PushDeclaration( const char* value ) +{ + SealElementIfJustOpened(); + if ( _textDepth < 0 && !_firstElement && !_compactMode) { + Putc( '\n' ); + PrintSpace( _depth ); + } + _firstElement = false; + + Write( "<?" ); + Write( value ); + Write( "?>" ); +} + + +void XMLPrinter::PushUnknown( const char* value ) +{ + SealElementIfJustOpened(); + if ( _textDepth < 0 && !_firstElement && !_compactMode) { + Putc( '\n' ); + PrintSpace( _depth ); + } + _firstElement = false; + + Write( "<!" ); + Write( value ); + Putc( '>' ); +} + + +bool XMLPrinter::VisitEnter( const XMLDocument& doc ) +{ + _processEntities = doc.ProcessEntities(); + if ( doc.HasBOM() ) { + PushHeader( true, false ); + } + return true; +} + + +bool XMLPrinter::VisitEnter( const XMLElement& element, const XMLAttribute* attribute ) +{ + const XMLElement* parentElem = nullptr; + if ( element.Parent() ) { + parentElem = element.Parent()->ToElement(); + } + const bool compactMode = parentElem ? CompactMode( *parentElem ) : _compactMode; + OpenElement( element.Name(), compactMode ); + while ( attribute ) { + PushAttribute( attribute->Name(), attribute->Value() ); + attribute = attribute->Next(); + } + return true; +} + + +bool XMLPrinter::VisitExit( const XMLElement& element ) +{ + CloseElement( CompactMode(element) ); + return true; +} + + +bool XMLPrinter::Visit( const XMLText& text ) +{ + PushText( text.Value(), text.CData() ); + return true; +} + + +bool XMLPrinter::Visit( const XMLComment& comment ) +{ + PushComment( comment.Value() ); + return true; +} + +bool XMLPrinter::Visit( const XMLDeclaration& declaration ) +{ + PushDeclaration( declaration.Value() ); + return true; +} + + +bool XMLPrinter::Visit( const XMLUnknown& unknown ) +{ + PushUnknown( unknown.Value() ); + return true; +} + +} // namespace tinyxml2 diff --git a/VirtualRobot/Util/xml/tinyxml2.h b/VirtualRobot/Util/xml/tinyxml2.h new file mode 100755 index 0000000000000000000000000000000000000000..ec4e1485d01567a36f43cd2340d12e6d7c12318c --- /dev/null +++ b/VirtualRobot/Util/xml/tinyxml2.h @@ -0,0 +1,2307 @@ +/* +Original code by Lee Thomason (www.grinninglizard.com) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must +not claim that you wrote the original software. If you use this +software in a product, an acknowledgment in the product documentation +would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. +*/ + +#ifndef TINYXML2_INCLUDED +#define TINYXML2_INCLUDED + +#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__) +# include <ctype.h> +# include <limits.h> +# include <stdio.h> +# include <stdlib.h> +# include <string.h> +# if defined(__PS3__) +# include <stddef.h> +# endif +#else +# include <cctype> +# include <climits> +# include <cstdio> +# include <cstdlib> +# include <cstring> +#endif +#include <stdint.h> + +/* + TODO: intern strings instead of allocation. +*/ +/* + gcc: + g++ -Wall -DTINYXML2_DEBUG tinyxml2.cpp xmltest.cpp -o gccxmltest.exe + + Formatting, Artistic Style: + AStyle.exe --style=1tbs --indent-switches --break-closing-brackets --indent-preprocessor tinyxml2.cpp tinyxml2.h +*/ + +#if defined( _DEBUG ) || defined (__DEBUG__) +# ifndef TINYXML2_DEBUG +# define TINYXML2_DEBUG +# endif +#endif + +#ifdef _MSC_VER +# pragma warning(push) +# pragma warning(disable: 4251) +#endif + +#ifdef _WIN32 +# ifdef TINYXML2_EXPORT +# define TINYXML2_LIB __declspec(dllexport) +# elif defined(TINYXML2_IMPORT) +# define TINYXML2_LIB __declspec(dllimport) +# else +# define TINYXML2_LIB +# endif +#elif __GNUC__ >= 4 +# define TINYXML2_LIB __attribute__((visibility("default"))) +#else +# define TINYXML2_LIB +#endif + + +#if defined(TINYXML2_DEBUG) +# if defined(_MSC_VER) +# // "(void)0," is for suppressing C4127 warning in "assert(false)", "assert(true)" and the like +# define TIXMLASSERT( x ) if ( !((void)0,(x))) { __debugbreak(); } +# elif defined (ANDROID_NDK) +# include <android/log.h> +# define TIXMLASSERT( x ) if ( !(x)) { __android_log_assert( "assert", "grinliz", "ASSERT in '%s' at %d.", __FILE__, __LINE__ ); } +# else +# include <assert.h> +# define TIXMLASSERT assert +# endif +#else +# define TIXMLASSERT( x ) {} +#endif + + +/* Versioning, past 1.0.14: + http://semver.org/ +*/ +static const int TIXML2_MAJOR_VERSION = 7; +static const int TIXML2_MINOR_VERSION = 0; +static const int TIXML2_PATCH_VERSION = 0; + +#define TINYXML2_MAJOR_VERSION 7 +#define TINYXML2_MINOR_VERSION 0 +#define TINYXML2_PATCH_VERSION 0 + +// A fixed element depth limit is problematic. There needs to be a +// limit to avoid a stack overflow. However, that limit varies per +// system, and the capacity of the stack. On the other hand, it's a trivial +// attack that can result from ill, malicious, or even correctly formed XML, +// so there needs to be a limit in place. +static const int TINYXML2_MAX_ELEMENT_DEPTH = 100; + +namespace tinyxml2 +{ +class XMLDocument; +class XMLElement; +class XMLAttribute; +class XMLComment; +class XMLText; +class XMLDeclaration; +class XMLUnknown; +class XMLPrinter; + +/* + A class that wraps strings. Normally stores the start and end + pointers into the XML file itself, and will apply normalization + and entity translation if actually read. Can also store (and memory + manage) a traditional char[] +*/ +class StrPair +{ +public: + enum { + NEEDS_ENTITY_PROCESSING = 0x01, + NEEDS_NEWLINE_NORMALIZATION = 0x02, + NEEDS_WHITESPACE_COLLAPSING = 0x04, + + TEXT_ELEMENT = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, + TEXT_ELEMENT_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, + ATTRIBUTE_NAME = 0, + ATTRIBUTE_VALUE = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION, + ATTRIBUTE_VALUE_LEAVE_ENTITIES = NEEDS_NEWLINE_NORMALIZATION, + COMMENT = NEEDS_NEWLINE_NORMALIZATION + }; + + StrPair() : _flags( 0 ), _start( 0 ), _end( 0 ) {} + ~StrPair(); + + void Set( char* start, char* end, int flags ) { + TIXMLASSERT( start ); + TIXMLASSERT( end ); + Reset(); + _start = start; + _end = end; + _flags = flags | NEEDS_FLUSH; + } + + const char* GetStr(); + + bool Empty() const { + return _start == _end; + } + + void SetInternedStr( const char* str ) { + Reset(); + _start = const_cast<char*>(str); + } + + void SetStr( const char* str, int flags=0 ); + + char* ParseText( char* in, const char* endTag, int strFlags, int* curLineNumPtr ); + char* ParseName( char* in ); + + void TransferTo( StrPair* other ); + void Reset(); + +private: + void CollapseWhitespace(); + + enum { + NEEDS_FLUSH = 0x100, + NEEDS_DELETE = 0x200 + }; + + int _flags; + char* _start; + char* _end; + + StrPair( const StrPair& other ); // not supported + void operator=( const StrPair& other ); // not supported, use TransferTo() +}; + + +/* + A dynamic array of Plain Old Data. Doesn't support constructors, etc. + Has a small initial memory pool, so that low or no usage will not + cause a call to new/delete +*/ +template <class T, int INITIAL_SIZE> +class DynArray +{ +public: + DynArray() : + _mem( _pool ), + _allocated( INITIAL_SIZE ), + _size( 0 ) + { + } + + ~DynArray() { + if ( _mem != _pool ) { + delete [] _mem; + } + } + + void Clear() { + _size = 0; + } + + void Push( T t ) { + TIXMLASSERT( _size < INT_MAX ); + EnsureCapacity( _size+1 ); + _mem[_size] = t; + ++_size; + } + + T* PushArr( int count ) { + TIXMLASSERT( count >= 0 ); + TIXMLASSERT( _size <= INT_MAX - count ); + EnsureCapacity( _size+count ); + T* ret = &_mem[_size]; + _size += count; + return ret; + } + + T Pop() { + TIXMLASSERT( _size > 0 ); + --_size; + return _mem[_size]; + } + + void PopArr( int count ) { + TIXMLASSERT( _size >= count ); + _size -= count; + } + + bool Empty() const { + return _size == 0; + } + + T& operator[](int i) { + TIXMLASSERT( i>= 0 && i < _size ); + return _mem[i]; + } + + const T& operator[](int i) const { + TIXMLASSERT( i>= 0 && i < _size ); + return _mem[i]; + } + + const T& PeekTop() const { + TIXMLASSERT( _size > 0 ); + return _mem[ _size - 1]; + } + + int Size() const { + TIXMLASSERT( _size >= 0 ); + return _size; + } + + int Capacity() const { + TIXMLASSERT( _allocated >= INITIAL_SIZE ); + return _allocated; + } + + void SwapRemove(int i) { + TIXMLASSERT(i >= 0 && i < _size); + TIXMLASSERT(_size > 0); + _mem[i] = _mem[_size - 1]; + --_size; + } + + const T* Mem() const { + TIXMLASSERT( _mem ); + return _mem; + } + + T* Mem() { + TIXMLASSERT( _mem ); + return _mem; + } + +private: + DynArray( const DynArray& ); // not supported + void operator=( const DynArray& ); // not supported + + void EnsureCapacity( int cap ) { + TIXMLASSERT( cap > 0 ); + if ( cap > _allocated ) { + TIXMLASSERT( cap <= INT_MAX / 2 ); + int newAllocated = cap * 2; + T* newMem = new T[newAllocated]; + TIXMLASSERT( newAllocated >= _size ); + memcpy( newMem, _mem, sizeof(T)*_size ); // warning: not using constructors, only works for PODs + if ( _mem != _pool ) { + delete [] _mem; + } + _mem = newMem; + _allocated = newAllocated; + } + } + + T* _mem; + T _pool[INITIAL_SIZE]; + int _allocated; // objects allocated + int _size; // number objects in use +}; + + +/* + Parent virtual class of a pool for fast allocation + and deallocation of objects. +*/ +class MemPool +{ +public: + MemPool() {} + virtual ~MemPool() {} + + virtual int ItemSize() const = 0; + virtual void* Alloc() = 0; + virtual void Free( void* ) = 0; + virtual void SetTracked() = 0; +}; + + +/* + Template child class to create pools of the correct type. +*/ +template< int ITEM_SIZE > +class MemPoolT : public MemPool +{ +public: + MemPoolT() : _blockPtrs(), _root(0), _currentAllocs(0), _nAllocs(0), _maxAllocs(0), _nUntracked(0) {} + ~MemPoolT() { + MemPoolT< ITEM_SIZE >::Clear(); + } + + void Clear() { + // Delete the blocks. + while( !_blockPtrs.Empty()) { + Block* lastBlock = _blockPtrs.Pop(); + delete lastBlock; + } + _root = 0; + _currentAllocs = 0; + _nAllocs = 0; + _maxAllocs = 0; + _nUntracked = 0; + } + + int ItemSize() const override { + return ITEM_SIZE; + } + int CurrentAllocs() const { + return _currentAllocs; + } + + void* Alloc() override { + if ( !_root ) { + // Need a new block. + Block* block = new Block(); + _blockPtrs.Push( block ); + + Item* blockItems = block->items; + for( int i = 0; i < ITEMS_PER_BLOCK - 1; ++i ) { + blockItems[i].next = &(blockItems[i + 1]); + } + blockItems[ITEMS_PER_BLOCK - 1].next = 0; + _root = blockItems; + } + Item* const result = _root; + TIXMLASSERT( result != 0 ); + _root = _root->next; + + ++_currentAllocs; + if ( _currentAllocs > _maxAllocs ) { + _maxAllocs = _currentAllocs; + } + ++_nAllocs; + ++_nUntracked; + return result; + } + + void Free( void* mem ) override { + if ( !mem ) { + return; + } + --_currentAllocs; + Item* item = static_cast<Item*>( mem ); +#ifdef TINYXML2_DEBUG + memset( item, 0xfe, sizeof( *item ) ); +#endif + item->next = _root; + _root = item; + } + void Trace( const char* name ) { + printf( "Mempool %s watermark=%d [%dk] current=%d size=%d nAlloc=%d blocks=%d\n", + name, _maxAllocs, _maxAllocs * ITEM_SIZE / 1024, _currentAllocs, + ITEM_SIZE, _nAllocs, _blockPtrs.Size() ); + } + + void SetTracked() override { + --_nUntracked; + } + + int Untracked() const { + return _nUntracked; + } + + // This number is perf sensitive. 4k seems like a good tradeoff on my machine. + // The test file is large, 170k. + // Release: VS2010 gcc(no opt) + // 1k: 4000 + // 2k: 4000 + // 4k: 3900 21000 + // 16k: 5200 + // 32k: 4300 + // 64k: 4000 21000 + // Declared public because some compilers do not accept to use ITEMS_PER_BLOCK + // in private part if ITEMS_PER_BLOCK is private + enum { ITEMS_PER_BLOCK = (4 * 1024) / ITEM_SIZE }; + +private: + MemPoolT( const MemPoolT& ); // not supported + void operator=( const MemPoolT& ); // not supported + + union Item { + Item* next; + char itemData[ITEM_SIZE]; + }; + struct Block { + Item items[ITEMS_PER_BLOCK]; + }; + DynArray< Block*, 10 > _blockPtrs; + Item* _root; + + int _currentAllocs; + int _nAllocs; + int _maxAllocs; + int _nUntracked; +}; + + + +/** + Implements the interface to the "Visitor pattern" (see the Accept() method.) + If you call the Accept() method, it requires being passed a XMLVisitor + class to handle callbacks. For nodes that contain other nodes (Document, Element) + you will get called with a VisitEnter/VisitExit pair. Nodes that are always leafs + are simply called with Visit(). + + If you return 'true' from a Visit method, recursive parsing will continue. If you return + false, <b>no children of this node or its siblings</b> will be visited. + + All flavors of Visit methods have a default implementation that returns 'true' (continue + visiting). You need to only override methods that are interesting to you. + + Generally Accept() is called on the XMLDocument, although all nodes support visiting. + + You should never change the document from a callback. + + @sa XMLNode::Accept() +*/ +class TINYXML2_LIB XMLVisitor +{ +public: + virtual ~XMLVisitor() {} + + /// Visit a document. + virtual bool VisitEnter( const XMLDocument& /*doc*/ ) { + return true; + } + /// Visit a document. + virtual bool VisitExit( const XMLDocument& /*doc*/ ) { + return true; + } + + /// Visit an element. + virtual bool VisitEnter( const XMLElement& /*element*/, const XMLAttribute* /*firstAttribute*/ ) { + return true; + } + /// Visit an element. + virtual bool VisitExit( const XMLElement& /*element*/ ) { + return true; + } + + /// Visit a declaration. + virtual bool Visit( const XMLDeclaration& /*declaration*/ ) { + return true; + } + /// Visit a text node. + virtual bool Visit( const XMLText& /*text*/ ) { + return true; + } + /// Visit a comment node. + virtual bool Visit( const XMLComment& /*comment*/ ) { + return true; + } + /// Visit an unknown node. + virtual bool Visit( const XMLUnknown& /*unknown*/ ) { + return true; + } +}; + +// WARNING: must match XMLDocument::_errorNames[] +enum XMLError { + XML_SUCCESS = 0, + XML_NO_ATTRIBUTE, + XML_WRONG_ATTRIBUTE_TYPE, + XML_ERROR_FILE_NOT_FOUND, + XML_ERROR_FILE_COULD_NOT_BE_OPENED, + XML_ERROR_FILE_READ_ERROR, + XML_ERROR_PARSING_ELEMENT, + XML_ERROR_PARSING_ATTRIBUTE, + XML_ERROR_PARSING_TEXT, + XML_ERROR_PARSING_CDATA, + XML_ERROR_PARSING_COMMENT, + XML_ERROR_PARSING_DECLARATION, + XML_ERROR_PARSING_UNKNOWN, + XML_ERROR_EMPTY_DOCUMENT, + XML_ERROR_MISMATCHED_ELEMENT, + XML_ERROR_PARSING, + XML_CAN_NOT_CONVERT_TEXT, + XML_NO_TEXT_NODE, + XML_ELEMENT_DEPTH_EXCEEDED, + + XML_ERROR_COUNT +}; + + +/* + Utility functionality. +*/ +class TINYXML2_LIB XMLUtil +{ +public: + static const char* SkipWhiteSpace( const char* p, int* curLineNumPtr ) { + TIXMLASSERT( p ); + + while( IsWhiteSpace(*p) ) { + if (curLineNumPtr && *p == '\n') { + ++(*curLineNumPtr); + } + ++p; + } + TIXMLASSERT( p ); + return p; + } + static char* SkipWhiteSpace( char* p, int* curLineNumPtr ) { + return const_cast<char*>( SkipWhiteSpace( const_cast<const char*>(p), curLineNumPtr ) ); + } + + // Anything in the high order range of UTF-8 is assumed to not be whitespace. This isn't + // correct, but simple, and usually works. + static bool IsWhiteSpace( char p ) { + return !IsUTF8Continuation(p) && isspace( static_cast<unsigned char>(p) ); + } + + inline static bool IsNameStartChar( unsigned char ch ) { + if ( ch >= 128 ) { + // This is a heuristic guess in attempt to not implement Unicode-aware isalpha() + return true; + } + if ( isalpha( ch ) ) { + return true; + } + return ch == ':' || ch == '_'; + } + + inline static bool IsNameChar( unsigned char ch ) { + return IsNameStartChar( ch ) + || isdigit( ch ) + || ch == '.' + || ch == '-'; + } + + inline static bool StringEqual( const char* p, const char* q, int nChar=INT_MAX ) { + if ( p == q ) { + return true; + } + TIXMLASSERT( p ); + TIXMLASSERT( q ); + TIXMLASSERT( nChar >= 0 ); + return strncmp( p, q, nChar ) == 0; + } + + inline static bool IsUTF8Continuation( char p ) { + return ( p & 0x80 ) != 0; + } + + static const char* ReadBOM( const char* p, bool* hasBOM ); + // p is the starting location, + // the UTF-8 value of the entity will be placed in value, and length filled in. + static const char* GetCharacterRef( const char* p, char* value, int* length ); + static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); + + // converts primitive types to strings + static void ToStr( int v, char* buffer, int bufferSize ); + static void ToStr( unsigned v, char* buffer, int bufferSize ); + static void ToStr( bool v, char* buffer, int bufferSize ); + static void ToStr( float v, char* buffer, int bufferSize ); + static void ToStr( double v, char* buffer, int bufferSize ); + static void ToStr(int64_t v, char* buffer, int bufferSize); + + // converts strings to primitive types + static bool ToInt( const char* str, int* value ); + static bool ToUnsigned( const char* str, unsigned* value ); + static bool ToBool( const char* str, bool* value ); + static bool ToFloat( const char* str, float* value ); + static bool ToDouble( const char* str, double* value ); + static bool ToInt64(const char* str, int64_t* value); + + // Changes what is serialized for a boolean value. + // Default to "true" and "false". Shouldn't be changed + // unless you have a special testing or compatibility need. + // Be careful: static, global, & not thread safe. + // Be sure to set static const memory as parameters. + static void SetBoolSerialization(const char* writeTrue, const char* writeFalse); + +private: + static const char* writeBoolTrue; + static const char* writeBoolFalse; +}; + + +/** XMLNode is a base class for every object that is in the + XML Document Object Model (DOM), except XMLAttributes. + Nodes have siblings, a parent, and children which can + be navigated. A node is always in a XMLDocument. + The type of a XMLNode can be queried, and it can + be cast to its more defined type. + + A XMLDocument allocates memory for all its Nodes. + When the XMLDocument gets deleted, all its Nodes + will also be deleted. + + @verbatim + A Document can contain: Element (container or leaf) + Comment (leaf) + Unknown (leaf) + Declaration( leaf ) + + An Element can contain: Element (container or leaf) + Text (leaf) + Attributes (not on tree) + Comment (leaf) + Unknown (leaf) + + @endverbatim +*/ +class TINYXML2_LIB XMLNode +{ + friend class XMLDocument; + friend class XMLElement; +public: + + /// Get the XMLDocument that owns this XMLNode. + const XMLDocument* GetDocument() const { + TIXMLASSERT( _document ); + return _document; + } + /// Get the XMLDocument that owns this XMLNode. + XMLDocument* GetDocument() { + TIXMLASSERT( _document ); + return _document; + } + + /// Safely cast to an Element, or null. + virtual XMLElement* ToElement() { + return 0; + } + /// Safely cast to Text, or null. + virtual XMLText* ToText() { + return 0; + } + /// Safely cast to a Comment, or null. + virtual XMLComment* ToComment() { + return 0; + } + /// Safely cast to a Document, or null. + virtual XMLDocument* ToDocument() { + return 0; + } + /// Safely cast to a Declaration, or null. + virtual XMLDeclaration* ToDeclaration() { + return 0; + } + /// Safely cast to an Unknown, or null. + virtual XMLUnknown* ToUnknown() { + return 0; + } + + virtual const XMLElement* ToElement() const { + return 0; + } + virtual const XMLText* ToText() const { + return 0; + } + virtual const XMLComment* ToComment() const { + return 0; + } + virtual const XMLDocument* ToDocument() const { + return 0; + } + virtual const XMLDeclaration* ToDeclaration() const { + return 0; + } + virtual const XMLUnknown* ToUnknown() const { + return 0; + } + + /** The meaning of 'value' changes for the specific type. + @verbatim + Document: empty (NULL is returned, not an empty string) + Element: name of the element + Comment: the comment text + Unknown: the tag contents + Text: the text string + @endverbatim + */ + const char* Value() const; + + /** Set the Value of an XML node. + @sa Value() + */ + void SetValue( const char* val, bool staticMem=false ); + + /// Gets the line number the node is in, if the document was parsed from a file. + int GetLineNum() const { return _parseLineNum; } + + /// Get the parent of this node on the DOM. + const XMLNode* Parent() const { + return _parent; + } + + XMLNode* Parent() { + return _parent; + } + + /// Returns true if this node has no children. + bool NoChildren() const { + return !_firstChild; + } + + /// Get the first child node, or null if none exists. + const XMLNode* FirstChild() const { + return _firstChild; + } + + XMLNode* FirstChild() { + return _firstChild; + } + + /** Get the first child element, or optionally the first child + element with the specified name. + */ + const XMLElement* FirstChildElement( const char* name = 0 ) const; + + XMLElement* FirstChildElement( const char* name = 0 ) { + return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->FirstChildElement( name )); + } + + /// Get the last child node, or null if none exists. + const XMLNode* LastChild() const { + return _lastChild; + } + + XMLNode* LastChild() { + return _lastChild; + } + + /** Get the last child element or optionally the last child + element with the specified name. + */ + const XMLElement* LastChildElement( const char* name = 0 ) const; + + XMLElement* LastChildElement( const char* name = 0 ) { + return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->LastChildElement(name) ); + } + + /// Get the previous (left) sibling node of this node. + const XMLNode* PreviousSibling() const { + return _prev; + } + + XMLNode* PreviousSibling() { + return _prev; + } + + /// Get the previous (left) sibling element of this node, with an optionally supplied name. + const XMLElement* PreviousSiblingElement( const char* name = 0 ) const ; + + XMLElement* PreviousSiblingElement( const char* name = 0 ) { + return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->PreviousSiblingElement( name ) ); + } + + /// Get the next (right) sibling node of this node. + const XMLNode* NextSibling() const { + return _next; + } + + XMLNode* NextSibling() { + return _next; + } + + /// Get the next (right) sibling element of this node, with an optionally supplied name. + const XMLElement* NextSiblingElement( const char* name = 0 ) const; + + XMLElement* NextSiblingElement( const char* name = 0 ) { + return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->NextSiblingElement( name ) ); + } + + /** + Add a child node as the last (right) child. + If the child node is already part of the document, + it is moved from its old location to the new location. + Returns the addThis argument or 0 if the node does not + belong to the same document. + */ + XMLNode* InsertEndChild( XMLNode* addThis ); + + XMLNode* LinkEndChild( XMLNode* addThis ) { + return InsertEndChild( addThis ); + } + /** + Add a child node as the first (left) child. + If the child node is already part of the document, + it is moved from its old location to the new location. + Returns the addThis argument or 0 if the node does not + belong to the same document. + */ + XMLNode* InsertFirstChild( XMLNode* addThis ); + /** + Add a node after the specified child node. + If the child node is already part of the document, + it is moved from its old location to the new location. + Returns the addThis argument or 0 if the afterThis node + is not a child of this node, or if the node does not + belong to the same document. + */ + XMLNode* InsertAfterChild( XMLNode* afterThis, XMLNode* addThis ); + + /** + Delete all the children of this node. + */ + void DeleteChildren(); + + /** + Delete a child of this node. + */ + void DeleteChild( XMLNode* node ); + + /** + Make a copy of this node, but not its children. + You may pass in a Document pointer that will be + the owner of the new Node. If the 'document' is + null, then the node returned will be allocated + from the current Document. (this->GetDocument()) + + Note: if called on a XMLDocument, this will return null. + */ + virtual XMLNode* ShallowClone( XMLDocument* document ) const = 0; + + /** + Make a copy of this node and all its children. + + If the 'target' is null, then the nodes will + be allocated in the current document. If 'target' + is specified, the memory will be allocated is the + specified XMLDocument. + + NOTE: This is probably not the correct tool to + copy a document, since XMLDocuments can have multiple + top level XMLNodes. You probably want to use + XMLDocument::DeepCopy() + */ + XMLNode* DeepClone( XMLDocument* target ) const; + + /** + Test if 2 nodes are the same, but don't test children. + The 2 nodes do not need to be in the same Document. + + Note: if called on a XMLDocument, this will return false. + */ + virtual bool ShallowEqual( const XMLNode* compare ) const = 0; + + /** Accept a hierarchical visit of the nodes in the TinyXML-2 DOM. Every node in the + XML tree will be conditionally visited and the host will be called back + via the XMLVisitor interface. + + This is essentially a SAX interface for TinyXML-2. (Note however it doesn't re-parse + the XML for the callbacks, so the performance of TinyXML-2 is unchanged by using this + interface versus any other.) + + The interface has been based on ideas from: + + - http://www.saxproject.org/ + - http://c2.com/cgi/wiki?HierarchicalVisitorPattern + + Which are both good references for "visiting". + + An example of using Accept(): + @verbatim + XMLPrinter printer; + tinyxmlDoc.Accept( &printer ); + const char* xmlcstr = printer.CStr(); + @endverbatim + */ + virtual bool Accept( XMLVisitor* visitor ) const = 0; + + /** + Set user data into the XMLNode. TinyXML-2 in + no way processes or interprets user data. + It is initially 0. + */ + void SetUserData(void* userData) { _userData = userData; } + + /** + Get user data set into the XMLNode. TinyXML-2 in + no way processes or interprets user data. + It is initially 0. + */ + void* GetUserData() const { return _userData; } + +protected: + explicit XMLNode( XMLDocument* ); + virtual ~XMLNode(); + + virtual char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr); + + XMLDocument* _document; + XMLNode* _parent; + mutable StrPair _value; + int _parseLineNum; + + XMLNode* _firstChild; + XMLNode* _lastChild; + + XMLNode* _prev; + XMLNode* _next; + + void* _userData; + +private: + MemPool* _memPool; + void Unlink( XMLNode* child ); + static void DeleteNode( XMLNode* node ); + void InsertChildPreamble( XMLNode* insertThis ) const; + const XMLElement* ToElementWithName( const char* name ) const; + + XMLNode( const XMLNode& ); // not supported + XMLNode& operator=( const XMLNode& ); // not supported +}; + + +/** XML text. + + Note that a text node can have child element nodes, for example: + @verbatim + <root>This is <b>bold</b></root> + @endverbatim + + A text node can have 2 ways to output the next. "normal" output + and CDATA. It will default to the mode it was parsed from the XML file and + you generally want to leave it alone, but you can change the output mode with + SetCData() and query it with CData(). +*/ +class TINYXML2_LIB XMLText : public XMLNode +{ + friend class XMLDocument; +public: + bool Accept( XMLVisitor* visitor ) const override; + + XMLText* ToText() override { + return this; + } + const XMLText* ToText() const override { + return this; + } + + /// Declare whether this should be CDATA or standard text. + void SetCData( bool isCData ) { + _isCData = isCData; + } + /// Returns true if this is a CDATA text element. + bool CData() const { + return _isCData; + } + + XMLNode* ShallowClone( XMLDocument* document ) const override; + bool ShallowEqual( const XMLNode* compare ) const override; + +protected: + explicit XMLText( XMLDocument* doc ) : XMLNode( doc ), _isCData( false ) {} + virtual ~XMLText() {} + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) override; + +private: + bool _isCData; + + XMLText( const XMLText& ); // not supported + XMLText& operator=( const XMLText& ); // not supported +}; + + +/** An XML Comment. */ +class TINYXML2_LIB XMLComment : public XMLNode +{ + friend class XMLDocument; +public: + XMLComment* ToComment() override { + return this; + } + const XMLComment* ToComment() const override { + return this; + } + + bool Accept( XMLVisitor* visitor ) const override; + + XMLNode* ShallowClone( XMLDocument* document ) const override; + bool ShallowEqual( const XMLNode* compare ) const override; + +protected: + explicit XMLComment( XMLDocument* doc ); + virtual ~XMLComment(); + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr) override; + +private: + XMLComment( const XMLComment& ); // not supported + XMLComment& operator=( const XMLComment& ); // not supported +}; + + +/** In correct XML the declaration is the first entry in the file. + @verbatim + <?xml version="1.0" standalone="yes"?> + @endverbatim + + TinyXML-2 will happily read or write files without a declaration, + however. + + The text of the declaration isn't interpreted. It is parsed + and written as a string. +*/ +class TINYXML2_LIB XMLDeclaration : public XMLNode +{ + friend class XMLDocument; +public: + XMLDeclaration* ToDeclaration() override { + return this; + } + const XMLDeclaration* ToDeclaration() const override { + return this; + } + + bool Accept( XMLVisitor* visitor ) const override; + + XMLNode* ShallowClone( XMLDocument* document ) const override; + bool ShallowEqual( const XMLNode* compare ) const override; + +protected: + explicit XMLDeclaration( XMLDocument* doc ); + virtual ~XMLDeclaration(); + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) override; + +private: + XMLDeclaration( const XMLDeclaration& ); // not supported + XMLDeclaration& operator=( const XMLDeclaration& ); // not supported +}; + + +/** Any tag that TinyXML-2 doesn't recognize is saved as an + unknown. It is a tag of text, but should not be modified. + It will be written back to the XML, unchanged, when the file + is saved. + + DTD tags get thrown into XMLUnknowns. +*/ +class TINYXML2_LIB XMLUnknown : public XMLNode +{ + friend class XMLDocument; +public: + XMLUnknown* ToUnknown() override { + return this; + } + const XMLUnknown* ToUnknown() const override { + return this; + } + + bool Accept( XMLVisitor* visitor ) const override; + + XMLNode* ShallowClone( XMLDocument* document ) const override; + bool ShallowEqual( const XMLNode* compare ) const override; + +protected: + explicit XMLUnknown( XMLDocument* doc ); + virtual ~XMLUnknown(); + + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) override; + +private: + XMLUnknown( const XMLUnknown& ); // not supported + XMLUnknown& operator=( const XMLUnknown& ); // not supported +}; + + + +/** An attribute is a name-value pair. Elements have an arbitrary + number of attributes, each with a unique name. + + @note The attributes are not XMLNodes. You may only query the + Next() attribute in a list. +*/ +class TINYXML2_LIB XMLAttribute +{ + friend class XMLElement; +public: + /// The name of the attribute. + const char* Name() const; + + /// The value of the attribute. + const char* Value() const; + + /// Gets the line number the attribute is in, if the document was parsed from a file. + int GetLineNum() const { return _parseLineNum; } + + /// The next attribute in the list. + const XMLAttribute* Next() const { + return _next; + } + + /** IntValue interprets the attribute as an integer, and returns the value. + If the value isn't an integer, 0 will be returned. There is no error checking; + use QueryIntValue() if you need error checking. + */ + int IntValue() const { + int i = 0; + QueryIntValue(&i); + return i; + } + + int64_t Int64Value() const { + int64_t i = 0; + QueryInt64Value(&i); + return i; + } + + /// Query as an unsigned integer. See IntValue() + unsigned UnsignedValue() const { + unsigned i=0; + QueryUnsignedValue( &i ); + return i; + } + /// Query as a boolean. See IntValue() + bool BoolValue() const { + bool b=false; + QueryBoolValue( &b ); + return b; + } + /// Query as a double. See IntValue() + double DoubleValue() const { + double d=0; + QueryDoubleValue( &d ); + return d; + } + /// Query as a float. See IntValue() + float FloatValue() const { + float f=0; + QueryFloatValue( &f ); + return f; + } + + /** QueryIntValue interprets the attribute as an integer, and returns the value + in the provided parameter. The function will return XML_SUCCESS on success, + and XML_WRONG_ATTRIBUTE_TYPE if the conversion is not successful. + */ + XMLError QueryIntValue( int* value ) const; + /// See QueryIntValue + XMLError QueryUnsignedValue( unsigned int* value ) const; + /// See QueryIntValue + XMLError QueryInt64Value(int64_t* value) const; + /// See QueryIntValue + XMLError QueryBoolValue( bool* value ) const; + /// See QueryIntValue + XMLError QueryDoubleValue( double* value ) const; + /// See QueryIntValue + XMLError QueryFloatValue( float* value ) const; + + /// Set the attribute to a string value. + void SetAttribute( const char* value ); + /// Set the attribute to value. + void SetAttribute( int value ); + /// Set the attribute to value. + void SetAttribute( unsigned value ); + /// Set the attribute to value. + void SetAttribute(int64_t value); + /// Set the attribute to value. + void SetAttribute( bool value ); + /// Set the attribute to value. + void SetAttribute( double value ); + /// Set the attribute to value. + void SetAttribute( float value ); + +private: + enum { BUF_SIZE = 200 }; + + XMLAttribute() : _name(), _value(),_parseLineNum( 0 ), _next( 0 ), _memPool( 0 ) {} + virtual ~XMLAttribute() {} + + XMLAttribute( const XMLAttribute& ); // not supported + void operator=( const XMLAttribute& ); // not supported + void SetName( const char* name ); + + char* ParseDeep( char* p, bool processEntities, int* curLineNumPtr ); + + mutable StrPair _name; + mutable StrPair _value; + int _parseLineNum; + XMLAttribute* _next; + MemPool* _memPool; +}; + + +/** The element is a container class. It has a value, the element name, + and can contain other elements, text, comments, and unknowns. + Elements also contain an arbitrary number of attributes. +*/ +class TINYXML2_LIB XMLElement : public XMLNode +{ + friend class XMLDocument; +public: + /// Get the name of an element (which is the Value() of the node.) + const char* Name() const { + return Value(); + } + /// Set the name of the element. + void SetName( const char* str, bool staticMem=false ) { + SetValue( str, staticMem ); + } + + XMLElement* ToElement() override { + return this; + } + const XMLElement* ToElement() const override{ + return this; + } + bool Accept( XMLVisitor* visitor ) const override; + + /** Given an attribute name, Attribute() returns the value + for the attribute of that name, or null if none + exists. For example: + + @verbatim + const char* value = ele->Attribute( "foo" ); + @endverbatim + + The 'value' parameter is normally null. However, if specified, + the attribute will only be returned if the 'name' and 'value' + match. This allow you to write code: + + @verbatim + if ( ele->Attribute( "foo", "bar" ) ) callFooIsBar(); + @endverbatim + + rather than: + @verbatim + if ( ele->Attribute( "foo" ) ) { + if ( strcmp( ele->Attribute( "foo" ), "bar" ) == 0 ) callFooIsBar(); + } + @endverbatim + */ + const char* Attribute( const char* name, const char* value=0 ) const; + + /** Given an attribute name, IntAttribute() returns the value + of the attribute interpreted as an integer. The default + value will be returned if the attribute isn't present, + or if there is an error. (For a method with error + checking, see QueryIntAttribute()). + */ + int IntAttribute(const char* name, int defaultValue = 0) const; + /// See IntAttribute() + unsigned UnsignedAttribute(const char* name, unsigned defaultValue = 0) const; + /// See IntAttribute() + int64_t Int64Attribute(const char* name, int64_t defaultValue = 0) const; + /// See IntAttribute() + bool BoolAttribute(const char* name, bool defaultValue = false) const; + /// See IntAttribute() + double DoubleAttribute(const char* name, double defaultValue = 0) const; + /// See IntAttribute() + float FloatAttribute(const char* name, float defaultValue = 0) const; + + /** Given an attribute name, QueryIntAttribute() returns + XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion + can't be performed, or XML_NO_ATTRIBUTE if the attribute + doesn't exist. If successful, the result of the conversion + will be written to 'value'. If not successful, nothing will + be written to 'value'. This allows you to provide default + value: + + @verbatim + int value = 10; + QueryIntAttribute( "foo", &value ); // if "foo" isn't found, value will still be 10 + @endverbatim + */ + XMLError QueryIntAttribute( const char* name, int* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryIntValue( value ); + } + + /// See QueryIntAttribute() + XMLError QueryUnsignedAttribute( const char* name, unsigned int* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryUnsignedValue( value ); + } + + /// See QueryIntAttribute() + XMLError QueryInt64Attribute(const char* name, int64_t* value) const { + const XMLAttribute* a = FindAttribute(name); + if (!a) { + return XML_NO_ATTRIBUTE; + } + return a->QueryInt64Value(value); + } + + /// See QueryIntAttribute() + XMLError QueryBoolAttribute( const char* name, bool* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryBoolValue( value ); + } + /// See QueryIntAttribute() + XMLError QueryDoubleAttribute( const char* name, double* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryDoubleValue( value ); + } + /// See QueryIntAttribute() + XMLError QueryFloatAttribute( const char* name, float* value ) const { + const XMLAttribute* a = FindAttribute( name ); + if ( !a ) { + return XML_NO_ATTRIBUTE; + } + return a->QueryFloatValue( value ); + } + + /// See QueryIntAttribute() + XMLError QueryStringAttribute(const char* name, const char** value) const { + const XMLAttribute* a = FindAttribute(name); + if (!a) { + return XML_NO_ATTRIBUTE; + } + *value = a->Value(); + return XML_SUCCESS; + } + + + + /** Given an attribute name, QueryAttribute() returns + XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion + can't be performed, or XML_NO_ATTRIBUTE if the attribute + doesn't exist. It is overloaded for the primitive types, + and is a generally more convenient replacement of + QueryIntAttribute() and related functions. + + If successful, the result of the conversion + will be written to 'value'. If not successful, nothing will + be written to 'value'. This allows you to provide default + value: + + @verbatim + int value = 10; + QueryAttribute( "foo", &value ); // if "foo" isn't found, value will still be 10 + @endverbatim + */ + XMLError QueryAttribute( const char* name, int* value ) const { + return QueryIntAttribute( name, value ); + } + + XMLError QueryAttribute( const char* name, unsigned int* value ) const { + return QueryUnsignedAttribute( name, value ); + } + + XMLError QueryAttribute(const char* name, int64_t* value) const { + return QueryInt64Attribute(name, value); + } + + XMLError QueryAttribute( const char* name, bool* value ) const { + return QueryBoolAttribute( name, value ); + } + + XMLError QueryAttribute( const char* name, double* value ) const { + return QueryDoubleAttribute( name, value ); + } + + XMLError QueryAttribute( const char* name, float* value ) const { + return QueryFloatAttribute( name, value ); + } + + /// Sets the named attribute to value. + void SetAttribute( const char* name, const char* value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, int value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, unsigned value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + + /// Sets the named attribute to value. + void SetAttribute(const char* name, int64_t value) { + XMLAttribute* a = FindOrCreateAttribute(name); + a->SetAttribute(value); + } + + /// Sets the named attribute to value. + void SetAttribute( const char* name, bool value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, double value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + /// Sets the named attribute to value. + void SetAttribute( const char* name, float value ) { + XMLAttribute* a = FindOrCreateAttribute( name ); + a->SetAttribute( value ); + } + + /** + Delete an attribute. + */ + void DeleteAttribute( const char* name ); + + /// Return the first attribute in the list. + const XMLAttribute* FirstAttribute() const { + return _rootAttribute; + } + /// Query a specific attribute in the list. + const XMLAttribute* FindAttribute( const char* name ) const; + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, GetText() is limited compared to getting the XMLText child + and accessing it directly. + + If the first child of 'this' is a XMLText, the GetText() + returns the character string of the Text node, else null is returned. + + This is a convenient method for getting the text of simple contained text: + @verbatim + <foo>This is text</foo> + const char* str = fooElement->GetText(); + @endverbatim + + 'str' will be a pointer to "This is text". + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + <foo><b>This is text</b></foo> + @endverbatim + + then the value of str would be null. The first child node isn't a text node, it is + another element. From this XML: + @verbatim + <foo>This is <b>text</b></foo> + @endverbatim + GetText() will return "This is ". + */ + const char* GetText() const; + + /** Convenience function for easy access to the text inside an element. Although easy + and concise, SetText() is limited compared to creating an XMLText child + and mutating it directly. + + If the first child of 'this' is a XMLText, SetText() sets its value to + the given string, otherwise it will create a first child that is an XMLText. + + This is a convenient method for setting the text of simple contained text: + @verbatim + <foo>This is text</foo> + fooElement->SetText( "Hullaballoo!" ); + <foo>Hullaballoo!</foo> + @endverbatim + + Note that this function can be misleading. If the element foo was created from + this XML: + @verbatim + <foo><b>This is text</b></foo> + @endverbatim + + then it will not change "This is text", but rather prefix it with a text element: + @verbatim + <foo>Hullaballoo!<b>This is text</b></foo> + @endverbatim + + For this XML: + @verbatim + <foo /> + @endverbatim + SetText() will generate + @verbatim + <foo>Hullaballoo!</foo> + @endverbatim + */ + void SetText( const char* inText ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( int value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( unsigned value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText(int64_t value); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( bool value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( double value ); + /// Convenience method for setting text inside an element. See SetText() for important limitations. + void SetText( float value ); + + /** + Convenience method to query the value of a child text node. This is probably best + shown by example. Given you have a document is this form: + @verbatim + <point> + <x>1</x> + <y>1.4</y> + </point> + @endverbatim + + The QueryIntText() and similar functions provide a safe and easier way to get to the + "value" of x and y. + + @verbatim + int x = 0; + float y = 0; // types of x and y are contrived for example + const XMLElement* xElement = pointElement->FirstChildElement( "x" ); + const XMLElement* yElement = pointElement->FirstChildElement( "y" ); + xElement->QueryIntText( &x ); + yElement->QueryFloatText( &y ); + @endverbatim + + @returns XML_SUCCESS (0) on success, XML_CAN_NOT_CONVERT_TEXT if the text cannot be converted + to the requested type, and XML_NO_TEXT_NODE if there is no child text to query. + + */ + XMLError QueryIntText( int* ival ) const; + /// See QueryIntText() + XMLError QueryUnsignedText( unsigned* uval ) const; + /// See QueryIntText() + XMLError QueryInt64Text(int64_t* uval) const; + /// See QueryIntText() + XMLError QueryBoolText( bool* bval ) const; + /// See QueryIntText() + XMLError QueryDoubleText( double* dval ) const; + /// See QueryIntText() + XMLError QueryFloatText( float* fval ) const; + + int IntText(int defaultValue = 0) const; + + /// See QueryIntText() + unsigned UnsignedText(unsigned defaultValue = 0) const; + /// See QueryIntText() + int64_t Int64Text(int64_t defaultValue = 0) const; + /// See QueryIntText() + bool BoolText(bool defaultValue = false) const; + /// See QueryIntText() + double DoubleText(double defaultValue = 0) const; + /// See QueryIntText() + float FloatText(float defaultValue = 0) const; + + // internal: + enum ElementClosingType { + OPEN, // <foo> + CLOSED, // <foo/> + CLOSING // </foo> + }; + ElementClosingType ClosingType() const { + return _closingType; + } + XMLNode* ShallowClone( XMLDocument* document ) const override; + bool ShallowEqual( const XMLNode* compare ) const override; + +protected: + char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr ) override; + +private: + XMLElement( XMLDocument* doc ); + virtual ~XMLElement(); + XMLElement( const XMLElement& ); // not supported + void operator=( const XMLElement& ); // not supported + + XMLAttribute* FindOrCreateAttribute( const char* name ); + char* ParseAttributes( char* p, int* curLineNumPtr ); + static void DeleteAttribute( XMLAttribute* attribute ); + XMLAttribute* CreateAttribute(); + + enum { BUF_SIZE = 200 }; + ElementClosingType _closingType; + // The attribute list is ordered; there is no 'lastAttribute' + // because the list needs to be scanned for dupes before adding + // a new attribute. + XMLAttribute* _rootAttribute; +}; + + +enum Whitespace { + PRESERVE_WHITESPACE, + COLLAPSE_WHITESPACE +}; + + +/** A Document binds together all the functionality. + It can be saved, loaded, and printed to the screen. + All Nodes are connected and allocated to a Document. + If the Document is deleted, all its Nodes are also deleted. +*/ +class TINYXML2_LIB XMLDocument : public XMLNode +{ + friend class XMLElement; + // Gives access to SetError and Push/PopDepth, but over-access for everything else. + // Wishing C++ had "internal" scope. + friend class XMLNode; + friend class XMLText; + friend class XMLComment; + friend class XMLDeclaration; + friend class XMLUnknown; +public: + /// constructor + XMLDocument( bool processEntities = true, Whitespace whitespaceMode = PRESERVE_WHITESPACE ); + ~XMLDocument(); + + XMLDocument* ToDocument() override { + TIXMLASSERT( this == _document ); + return this; + } + const XMLDocument* ToDocument() const override { + TIXMLASSERT( this == _document ); + return this; + } + + /** + Parse an XML file from a character string. + Returns XML_SUCCESS (0) on success, or + an errorID. + + You may optionally pass in the 'nBytes', which is + the number of bytes which will be parsed. If not + specified, TinyXML-2 will assume 'xml' points to a + null terminated string. + */ + XMLError Parse( const char* xml, size_t nBytes=(size_t)(-1) ); + + /** + Load an XML file from disk. + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError LoadFile( const char* filename ); + + /** + Load an XML file from disk. You are responsible + for providing and closing the FILE*. + + NOTE: The file should be opened as binary ("rb") + not text in order for TinyXML-2 to correctly + do newline normalization. + + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError LoadFile( FILE* ); + + /** + Save the XML file to disk. + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError SaveFile( const char* filename, bool compact = false ); + + /** + Save the XML file to disk. You are responsible + for providing and closing the FILE*. + + Returns XML_SUCCESS (0) on success, or + an errorID. + */ + XMLError SaveFile( FILE* fp, bool compact = false ); + + bool ProcessEntities() const { + return _processEntities; + } + Whitespace WhitespaceMode() const { + return _whitespaceMode; + } + + /** + Returns true if this document has a leading Byte Order Mark of UTF8. + */ + bool HasBOM() const { + return _writeBOM; + } + /** Sets whether to write the BOM when writing the file. + */ + void SetBOM( bool useBOM ) { + _writeBOM = useBOM; + } + + /** Return the root element of DOM. Equivalent to FirstChildElement(). + To get the first node, use FirstChild(). + */ + XMLElement* RootElement() { + return FirstChildElement(); + } + const XMLElement* RootElement() const { + return FirstChildElement(); + } + + /** Print the Document. If the Printer is not provided, it will + print to stdout. If you provide Printer, this can print to a file: + @verbatim + XMLPrinter printer( fp ); + doc.Print( &printer ); + @endverbatim + + Or you can use a printer to print to memory: + @verbatim + XMLPrinter printer; + doc.Print( &printer ); + // printer.CStr() has a const char* to the XML + @endverbatim + */ + void Print( XMLPrinter* streamer=0 ) const; + bool Accept( XMLVisitor* visitor ) const override; + + /** + Create a new Element associated with + this Document. The memory for the Element + is managed by the Document. + */ + XMLElement* NewElement( const char* name ); + /** + Create a new Comment associated with + this Document. The memory for the Comment + is managed by the Document. + */ + XMLComment* NewComment( const char* comment ); + /** + Create a new Text associated with + this Document. The memory for the Text + is managed by the Document. + */ + XMLText* NewText( const char* text ); + /** + Create a new Declaration associated with + this Document. The memory for the object + is managed by the Document. + + If the 'text' param is null, the standard + declaration is used.: + @verbatim + <?xml version="1.0" encoding="UTF-8"?> + @endverbatim + */ + XMLDeclaration* NewDeclaration( const char* text=0 ); + /** + Create a new Unknown associated with + this Document. The memory for the object + is managed by the Document. + */ + XMLUnknown* NewUnknown( const char* text ); + + /** + Delete a node associated with this document. + It will be unlinked from the DOM. + */ + void DeleteNode( XMLNode* node ); + + void ClearError() { + SetError(XML_SUCCESS, 0, 0); + } + + /// Return true if there was an error parsing the document. + bool Error() const { + return _errorID != XML_SUCCESS; + } + /// Return the errorID. + XMLError ErrorID() const { + return _errorID; + } + const char* ErrorName() const; + static const char* ErrorIDToName(XMLError errorID); + + /** Returns a "long form" error description. A hopefully helpful + diagnostic with location, line number, and/or additional info. + */ + const char* ErrorStr() const; + + /// A (trivial) utility function that prints the ErrorStr() to stdout. + void PrintError() const; + + /// Return the line where the error occurred, or zero if unknown. + int ErrorLineNum() const + { + return _errorLineNum; + } + + /// Clear the document, resetting it to the initial state. + void Clear(); + + /** + Copies this document to a target document. + The target will be completely cleared before the copy. + If you want to copy a sub-tree, see XMLNode::DeepClone(). + + NOTE: that the 'target' must be non-null. + */ + void DeepCopy(XMLDocument* target) const; + + // internal + char* Identify( char* p, XMLNode** node ); + + // internal + void MarkInUse(XMLNode*); + + XMLNode* ShallowClone( XMLDocument* /*document*/ ) const override { + return 0; + } + bool ShallowEqual( const XMLNode* /*compare*/ ) const override { + return false; + } + +private: + XMLDocument( const XMLDocument& ); // not supported + void operator=( const XMLDocument& ); // not supported + + bool _writeBOM; + bool _processEntities; + XMLError _errorID; + Whitespace _whitespaceMode; + mutable StrPair _errorStr; + int _errorLineNum; + char* _charBuffer; + int _parseCurLineNum; + int _parsingDepth; + // Memory tracking does add some overhead. + // However, the code assumes that you don't + // have a bunch of unlinked nodes around. + // Therefore it takes less memory to track + // in the document vs. a linked list in the XMLNode, + // and the performance is the same. + DynArray<XMLNode*, 10> _unlinked; + + MemPoolT< sizeof(XMLElement) > _elementPool; + MemPoolT< sizeof(XMLAttribute) > _attributePool; + MemPoolT< sizeof(XMLText) > _textPool; + MemPoolT< sizeof(XMLComment) > _commentPool; + + static const char* _errorNames[XML_ERROR_COUNT]; + + void Parse(); + + void SetError( XMLError error, int lineNum, const char* format, ... ); + + // Something of an obvious security hole, once it was discovered. + // Either an ill-formed XML or an excessively deep one can overflow + // the stack. Track stack depth, and error out if needed. + class DepthTracker { + public: + explicit DepthTracker(XMLDocument * document) { + this->_document = document; + document->PushDepth(); + } + ~DepthTracker() { + _document->PopDepth(); + } + private: + XMLDocument * _document; + }; + void PushDepth(); + void PopDepth(); + + template<class NodeType, int PoolElementSize> + NodeType* CreateUnlinkedNode( MemPoolT<PoolElementSize>& pool ); +}; + +template<class NodeType, int PoolElementSize> +inline NodeType* XMLDocument::CreateUnlinkedNode( MemPoolT<PoolElementSize>& pool ) +{ + TIXMLASSERT( sizeof( NodeType ) == PoolElementSize ); + TIXMLASSERT( sizeof( NodeType ) == pool.ItemSize() ); + NodeType* returnNode = new (pool.Alloc()) NodeType( this ); + TIXMLASSERT( returnNode ); + returnNode->_memPool = &pool; + + _unlinked.Push(returnNode); + return returnNode; +} + +/** + A XMLHandle is a class that wraps a node pointer with null checks; this is + an incredibly useful thing. Note that XMLHandle is not part of the TinyXML-2 + DOM structure. It is a separate utility class. + + Take an example: + @verbatim + <Document> + <Element attributeA = "valueA"> + <Child attributeB = "value1" /> + <Child attributeB = "value2" /> + </Element> + </Document> + @endverbatim + + Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very + easy to write a *lot* of code that looks like: + + @verbatim + XMLElement* root = document.FirstChildElement( "Document" ); + if ( root ) + { + XMLElement* element = root->FirstChildElement( "Element" ); + if ( element ) + { + XMLElement* child = element->FirstChildElement( "Child" ); + if ( child ) + { + XMLElement* child2 = child->NextSiblingElement( "Child" ); + if ( child2 ) + { + // Finally do something useful. + @endverbatim + + And that doesn't even cover "else" cases. XMLHandle addresses the verbosity + of such code. A XMLHandle checks for null pointers so it is perfectly safe + and correct to use: + + @verbatim + XMLHandle docHandle( &document ); + XMLElement* child2 = docHandle.FirstChildElement( "Document" ).FirstChildElement( "Element" ).FirstChildElement().NextSiblingElement(); + if ( child2 ) + { + // do something useful + @endverbatim + + Which is MUCH more concise and useful. + + It is also safe to copy handles - internally they are nothing more than node pointers. + @verbatim + XMLHandle handleCopy = handle; + @endverbatim + + See also XMLConstHandle, which is the same as XMLHandle, but operates on const objects. +*/ +class TINYXML2_LIB XMLHandle +{ +public: + /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. + explicit XMLHandle( XMLNode* node ) : _node( node ) { + } + /// Create a handle from a node. + explicit XMLHandle( XMLNode& node ) : _node( &node ) { + } + /// Copy constructor + XMLHandle( const XMLHandle& ref ) : _node( ref._node ) { + } + /// Assignment + XMLHandle& operator=( const XMLHandle& ref ) { + _node = ref._node; + return *this; + } + + /// Get the first child of this handle. + XMLHandle FirstChild() { + return XMLHandle( _node ? _node->FirstChild() : 0 ); + } + /// Get the first child element of this handle. + XMLHandle FirstChildElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->FirstChildElement( name ) : 0 ); + } + /// Get the last child of this handle. + XMLHandle LastChild() { + return XMLHandle( _node ? _node->LastChild() : 0 ); + } + /// Get the last child element of this handle. + XMLHandle LastChildElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->LastChildElement( name ) : 0 ); + } + /// Get the previous sibling of this handle. + XMLHandle PreviousSibling() { + return XMLHandle( _node ? _node->PreviousSibling() : 0 ); + } + /// Get the previous sibling element of this handle. + XMLHandle PreviousSiblingElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->PreviousSiblingElement( name ) : 0 ); + } + /// Get the next sibling of this handle. + XMLHandle NextSibling() { + return XMLHandle( _node ? _node->NextSibling() : 0 ); + } + /// Get the next sibling element of this handle. + XMLHandle NextSiblingElement( const char* name = 0 ) { + return XMLHandle( _node ? _node->NextSiblingElement( name ) : 0 ); + } + + /// Safe cast to XMLNode. This can return null. + XMLNode* ToNode() { + return _node; + } + /// Safe cast to XMLElement. This can return null. + XMLElement* ToElement() { + return ( _node ? _node->ToElement() : 0 ); + } + /// Safe cast to XMLText. This can return null. + XMLText* ToText() { + return ( _node ? _node->ToText() : 0 ); + } + /// Safe cast to XMLUnknown. This can return null. + XMLUnknown* ToUnknown() { + return ( _node ? _node->ToUnknown() : 0 ); + } + /// Safe cast to XMLDeclaration. This can return null. + XMLDeclaration* ToDeclaration() { + return ( _node ? _node->ToDeclaration() : 0 ); + } + +private: + XMLNode* _node; +}; + + +/** + A variant of the XMLHandle class for working with const XMLNodes and Documents. It is the + same in all regards, except for the 'const' qualifiers. See XMLHandle for API. +*/ +class TINYXML2_LIB XMLConstHandle +{ +public: + explicit XMLConstHandle( const XMLNode* node ) : _node( node ) { + } + explicit XMLConstHandle( const XMLNode& node ) : _node( &node ) { + } + XMLConstHandle( const XMLConstHandle& ref ) : _node( ref._node ) { + } + + XMLConstHandle& operator=( const XMLConstHandle& ref ) { + _node = ref._node; + return *this; + } + + const XMLConstHandle FirstChild() const { + return XMLConstHandle( _node ? _node->FirstChild() : 0 ); + } + const XMLConstHandle FirstChildElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->FirstChildElement( name ) : 0 ); + } + const XMLConstHandle LastChild() const { + return XMLConstHandle( _node ? _node->LastChild() : 0 ); + } + const XMLConstHandle LastChildElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->LastChildElement( name ) : 0 ); + } + const XMLConstHandle PreviousSibling() const { + return XMLConstHandle( _node ? _node->PreviousSibling() : 0 ); + } + const XMLConstHandle PreviousSiblingElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->PreviousSiblingElement( name ) : 0 ); + } + const XMLConstHandle NextSibling() const { + return XMLConstHandle( _node ? _node->NextSibling() : 0 ); + } + const XMLConstHandle NextSiblingElement( const char* name = 0 ) const { + return XMLConstHandle( _node ? _node->NextSiblingElement( name ) : 0 ); + } + + + const XMLNode* ToNode() const { + return _node; + } + const XMLElement* ToElement() const { + return ( _node ? _node->ToElement() : 0 ); + } + const XMLText* ToText() const { + return ( _node ? _node->ToText() : 0 ); + } + const XMLUnknown* ToUnknown() const { + return ( _node ? _node->ToUnknown() : 0 ); + } + const XMLDeclaration* ToDeclaration() const { + return ( _node ? _node->ToDeclaration() : 0 ); + } + +private: + const XMLNode* _node; +}; + + +/** + Printing functionality. The XMLPrinter gives you more + options than the XMLDocument::Print() method. + + It can: + -# Print to memory. + -# Print to a file you provide. + -# Print XML without a XMLDocument. + + Print to Memory + + @verbatim + XMLPrinter printer; + doc.Print( &printer ); + SomeFunction( printer.CStr() ); + @endverbatim + + Print to a File + + You provide the file pointer. + @verbatim + XMLPrinter printer( fp ); + doc.Print( &printer ); + @endverbatim + + Print without a XMLDocument + + When loading, an XML parser is very useful. However, sometimes + when saving, it just gets in the way. The code is often set up + for streaming, and constructing the DOM is just overhead. + + The Printer supports the streaming case. The following code + prints out a trivially simple XML file without ever creating + an XML document. + + @verbatim + XMLPrinter printer( fp ); + printer.OpenElement( "foo" ); + printer.PushAttribute( "foo", "bar" ); + printer.CloseElement(); + @endverbatim +*/ +class TINYXML2_LIB XMLPrinter : public XMLVisitor +{ +public: + /** Construct the printer. If the FILE* is specified, + this will print to the FILE. Else it will print + to memory, and the result is available in CStr(). + If 'compact' is set to true, then output is created + with only required whitespace and newlines. + */ + XMLPrinter( FILE* file=0, bool compact = false, int depth = 0 ); + virtual ~XMLPrinter() {} + + /** If streaming, write the BOM and declaration. */ + void PushHeader( bool writeBOM, bool writeDeclaration ); + /** If streaming, start writing an element. + The element must be closed with CloseElement() + */ + void OpenElement( const char* name, bool compactMode=false ); + /// If streaming, add an attribute to an open element. + void PushAttribute( const char* name, const char* value ); + void PushAttribute( const char* name, int value ); + void PushAttribute( const char* name, unsigned value ); + void PushAttribute(const char* name, int64_t value); + void PushAttribute( const char* name, bool value ); + void PushAttribute( const char* name, double value ); + /// If streaming, close the Element. + virtual void CloseElement( bool compactMode=false ); + + /// Add a text node. + void PushText( const char* text, bool cdata=false ); + /// Add a text node from an integer. + void PushText( int value ); + /// Add a text node from an unsigned. + void PushText( unsigned value ); + /// Add a text node from an unsigned. + void PushText(int64_t value); + /// Add a text node from a bool. + void PushText( bool value ); + /// Add a text node from a float. + void PushText( float value ); + /// Add a text node from a double. + void PushText( double value ); + + /// Add a comment + void PushComment( const char* comment ); + + void PushDeclaration( const char* value ); + void PushUnknown( const char* value ); + + bool VisitEnter( const XMLDocument& /*doc*/ ) override; + bool VisitExit( const XMLDocument& /*doc*/ ) override { + return true; + } + + bool VisitEnter( const XMLElement& element, const XMLAttribute* attribute ) override; + bool VisitExit( const XMLElement& element ) override; + + bool Visit( const XMLText& text ) override; + bool Visit( const XMLComment& comment ) override; + bool Visit( const XMLDeclaration& declaration ) override; + bool Visit( const XMLUnknown& unknown ) override; + + /** + If in print to memory mode, return a pointer to + the XML file in memory. + */ + const char* CStr() const { + return _buffer.Mem(); + } + /** + If in print to memory mode, return the size + of the XML file in memory. (Note the size returned + includes the terminating null.) + */ + int CStrSize() const { + return _buffer.Size(); + } + /** + If in print to memory mode, reset the buffer to the + beginning. + */ + void ClearBuffer() { + _buffer.Clear(); + _buffer.Push(0); + _firstElement = true; + } + +protected: + virtual bool CompactMode( const XMLElement& ) { return _compactMode; } + + /** Prints out the space before an element. You may override to change + the space and tabs used. A PrintSpace() override should call Print(). + */ + virtual void PrintSpace( int depth ); + void Print( const char* format, ... ); + void Write( const char* data, size_t size ); + inline void Write( const char* data ) { Write( data, strlen( data ) ); } + void Putc( char ch ); + + void SealElementIfJustOpened(); + bool _elementJustOpened; + DynArray< const char*, 10 > _stack; + +private: + void PrintString( const char*, bool restrictedEntitySet ); // prints out, after detecting entities. + + bool _firstElement; + FILE* _fp; + int _depth; + int _textDepth; + bool _processEntities; + bool _compactMode; + + enum { + ENTITY_RANGE = 64, + BUF_SIZE = 200 + }; + bool _entityFlag[ENTITY_RANGE]; + bool _restrictedEntityFlag[ENTITY_RANGE]; + + DynArray< char, 20 > _buffer; + + // Prohibit cloning, intentionally not implemented + XMLPrinter( const XMLPrinter& ); + XMLPrinter& operator=( const XMLPrinter& ); +}; + + +} // tinyxml2 + +#if defined(_MSC_VER) +# pragma warning(pop) +#endif + +#endif // TINYXML2_INCLUDED diff --git a/VirtualRobot/VirtualRobotException.cpp b/VirtualRobot/VirtualRobotException.cpp index 1a7faeea191b783da3e402989e07107ef5d8c6de..916c733d5dc355527472c6f7d3003e83e918e9c7 100644 --- a/VirtualRobot/VirtualRobotException.cpp +++ b/VirtualRobot/VirtualRobotException.cpp @@ -17,8 +17,7 @@ namespace VirtualRobot } VirtualRobotException::~VirtualRobotException() throw() - { - } + = default; const char* VirtualRobotException::what() const throw() { diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp index 210bd9d70b369899a8dc89ad3e9f6a12b99e1fb7..ab0de471fd232bc3bd2c15774ec4a18b3f03cb7e 100644 --- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp +++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp @@ -26,17 +26,17 @@ namespace VirtualRobot CoinVisualization::CoinVisualization(const VisualizationNodePtr visualizationNode) : Visualization(visualizationNode) { - selection = NULL; - visuRoot = NULL; - color = NULL; + selection = nullptr; + visuRoot = nullptr; + color = nullptr; } CoinVisualization::CoinVisualization(const std::vector<VisualizationNodePtr>& visualizationNodes) : Visualization(visualizationNodes) { - selection = NULL; - visuRoot = NULL; - color = NULL; + selection = nullptr; + visuRoot = nullptr; + color = nullptr; } CoinVisualization::~CoinVisualization() diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp index 4ac95d152bf8b865b8d88a4701c23f0842750730..995ce0bc62dbbf735384277005109bf6eb793a20 100644 --- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp +++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp @@ -95,13 +95,11 @@ namespace VirtualRobot CoinVisualizationFactory::TextureCacheMap CoinVisualizationFactory::globalTextureCache; CoinVisualizationFactory::CoinVisualizationFactory() - { - } + = default; CoinVisualizationFactory::~CoinVisualizationFactory() - { - } + = default; void CoinVisualizationFactory::init(int &argc, char* argv[], const std::string &appName) { @@ -144,9 +142,8 @@ namespace VirtualRobot Eigen::Matrix4f currentTransform = Eigen::Matrix4f::Identity(); - for (std::vector<Primitive::PrimitivePtr>::const_iterator it = primitives.begin(); it != primitives.end(); it++) + for (auto p : primitives) { - Primitive::PrimitivePtr p = *it; currentTransform *= p->transform; SoSeparator* soSep = new SoSeparator(); SoNode* pNode = GetNodeFromPrimitive(p, boundingBox, color); @@ -362,7 +359,7 @@ namespace VirtualRobot SoNode* coinVisualization = SoDB::readAll(&soInput); // check if the visualization was read - if (NULL == coinVisualization) + if (nullptr == coinVisualization) { std::cerr << "Problem reading model from SoInput: " << soInput.getCurFileName() << std::endl; return; @@ -802,7 +799,7 @@ namespace VirtualRobot result->addChild(tmp2); } - if (text != NULL) + if (text != nullptr) { SoSeparator* textSep = new SoSeparator(); SoTranslation* moveT = new SoTranslation(); @@ -955,7 +952,7 @@ namespace VirtualRobot SoUnits* u = new SoUnits(); u->units = SoUnits::MILLIMETERS; res->addChild(u); - res->addChild(CreateEllipse(x, y, z, NULL, showAxes, axesHeight, axesWidth)); + res->addChild(CreateEllipse(x, y, z, nullptr, showAxes, axesHeight, axesWidth)); VisualizationNodePtr visualizationNode(new CoinVisualizationNode(res)); res->unref(); return visualizationNode; @@ -1105,7 +1102,7 @@ namespace VirtualRobot } private: - ~DeleteTextureCallBack() override{} + ~DeleteTextureCallBack() override= default; std::string nodeName; std::string path; size_t filesize; @@ -1538,9 +1535,9 @@ namespace VirtualRobot SoSeparator* res = new SoSeparator; res->ref(); - for (size_t i = 0; i < contacts.size(); i++) + for (auto & contact : contacts) { - res->addChild(getCoinVisualization(contacts[i], frictionConeHeight, frictionConeRadius, scaleAccordingToApproachDir)); + res->addChild(getCoinVisualization(contact, frictionConeHeight, frictionConeRadius, scaleAccordingToApproachDir)); } res->unrefNoDelete(); @@ -2351,7 +2348,7 @@ namespace VirtualRobot if (!reachSpace || reachSpace->numVoxels[0] <= 0 || reachSpace->numVoxels[1] <= 0 || reachSpace->numVoxels[2] <= 0) { - return NULL; + return nullptr; } //float x[6]; @@ -2416,7 +2413,7 @@ namespace VirtualRobot if (entryRot.size() == 0) { - return NULL; + return nullptr; } SoSeparator* res = new SoSeparator; @@ -3636,7 +3633,7 @@ namespace VirtualRobot bool CoinVisualizationFactory::renderOffscreen(SoOffscreenRenderer* renderer, SoPerspectiveCamera* cam, SoNode* scene, unsigned char** buffer) { //SbTime t1 = SbTime::getTimeOfDay(); // for profiling - if (!renderer || !cam || !scene || buffer == NULL) + if (!renderer || !cam || !scene || buffer == nullptr) { return false; } @@ -4031,9 +4028,9 @@ namespace VirtualRobot std::vector<MathTools::Segment> s = oobb.getSegments(); - for (size_t i = 0; i < s.size(); i++) + for (const auto & i : s) { - sep->addChild(CreateSegmentVisualization(s[i], colorLine, lineSize)); + sep->addChild(CreateSegmentVisualization(i, colorLine, lineSize)); } res->addChild(sep); @@ -4304,7 +4301,7 @@ namespace VirtualRobot { if (!n) { - return NULL; + return nullptr; } bool copyImages = true; @@ -4342,10 +4339,9 @@ namespace VirtualRobot SoNode* result = n->copy(TRUE); // reset the changed ones back - for (std::vector<SoSFImage*>::iterator it = changedImages.begin(); - it != changedImages.end(); it++) + for (auto & changedImage : changedImages) { - (*it)->setDefault(TRUE); + changedImage->setDefault(TRUE); } return result; diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp index 662c404ec62c87aa8363f32a764c8abf49adb8c6..256773dc1d377033df51149a2ed88b7491c4498b 100644 --- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp +++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp @@ -311,7 +311,7 @@ namespace VirtualRobot { THROW_VR_EXCEPTION_IF(scaling <= 0, "Scaling must be >0"); - SoSeparator* newModel = NULL; + SoSeparator* newModel = nullptr; if (visualization) { diff --git a/VirtualRobot/Visualization/ColorMap.cpp b/VirtualRobot/Visualization/ColorMap.cpp index facb2597891c5662a599f19d57d3bfefb9a73c8b..aeda2984eb5a034b2aa800c04e1e700a4cfdd2a3 100644 --- a/VirtualRobot/Visualization/ColorMap.cpp +++ b/VirtualRobot/Visualization/ColorMap.cpp @@ -19,13 +19,10 @@ namespace VirtualRobot } ColorMap::ColorMap() - { - } + = default; ColorMap::~ColorMap() - { - - } + = default; void ColorMap::create(type t) { colorKeys.clear(); diff --git a/VirtualRobot/Visualization/TriMeshModel.cpp b/VirtualRobot/Visualization/TriMeshModel.cpp index 941ab1a65ff4eec805c015e08de23ec6fa91c085..728e39dad9079c16ea22e98b72f2a66348e701c8 100644 --- a/VirtualRobot/Visualization/TriMeshModel.cpp +++ b/VirtualRobot/Visualization/TriMeshModel.cpp @@ -20,14 +20,13 @@ namespace VirtualRobot TriMeshModel::TriMeshModel() - { - } + = default; TriMeshModel::TriMeshModel(std::vector <triangle>& triangles) { - for (size_t i = 0; i < triangles.size(); i++) + for (auto & triangle : triangles) { - addTriangleWithFace(triangles[i].vertex1, triangles[i].vertex2, triangles[i].vertex3); + addTriangleWithFace(triangle.vertex1, triangle.vertex2, triangle.vertex3); } } @@ -360,7 +359,7 @@ namespace VirtualRobot vertices.at(i)[1], vertices.at(i)[2]}); } - typedef float num_t; + using num_t = float; // construct a kd-tree index: typedef nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor<num_t, PointCloud<num_t> > , @@ -794,9 +793,9 @@ namespace VirtualRobot { cout << "TriMeshModel Normals:" << endl; std::streamsize pr = cout.precision(2); - for (size_t i = 0; i < faces.size(); i++) + for (auto & face : faces) { - cout << "<" << faces[i].normal(0) << "," << faces[i].normal(1) << "," << faces[i].normal(2) << ">,"; + cout << "<" << face.normal(0) << "," << face.normal(1) << "," << face.normal(2) << ">,"; } cout.precision(pr); } @@ -816,15 +815,15 @@ namespace VirtualRobot { cout << "TriMeshModel Faces (vertex indices):" << endl; std::streamsize pr = cout.precision(2); - for (size_t i = 0; i < faces.size(); i++) + for (auto & face : faces) { - cout << faces[i].id1 << "," << faces[i].id2 << "," << faces[i].id3 << endl; + cout << face.id1 << "," << face.id2 << "," << face.id3 << endl; } cout.precision(pr); } - void TriMeshModel::scale(Eigen::Vector3f& scaleFactor) + void TriMeshModel::scale(const Eigen::Vector3f& scaleFactor) { if (scaleFactor(0) == 1.0f && scaleFactor(1) == 1.0f && scaleFactor(2) == 1.0f) { @@ -849,7 +848,7 @@ namespace VirtualRobot return clone(scaleFactor); } - VirtualRobot::TriMeshModelPtr TriMeshModel::clone(Eigen::Vector3f& scaleFactor) const + VirtualRobot::TriMeshModelPtr TriMeshModel::clone(const Eigen::Vector3f& scaleFactor) const { TriMeshModelPtr r(new TriMeshModel()); r->vertices = vertices; diff --git a/VirtualRobot/Visualization/TriMeshModel.h b/VirtualRobot/Visualization/TriMeshModel.h index e410f1413243a0339f74a89fc1cd979795bfa69c..86da123626fd73047fe96ab19324d999f81fe700 100644 --- a/VirtualRobot/Visualization/TriMeshModel.h +++ b/VirtualRobot/Visualization/TriMeshModel.h @@ -126,9 +126,9 @@ namespace VirtualRobot bool checkFacesHaveSameEdge(const MathTools::TriangleFace& face1, const MathTools::TriangleFace& face2, std::vector<std::pair<int, int> >& commonVertexIds) const; unsigned int checkAndCorrectNormals(bool inverted); - virtual void scale(Eigen::Vector3f& scaleFactor); + virtual void scale(const Eigen::Vector3f& scaleFactor); TriMeshModelPtr clone() const; - TriMeshModelPtr clone(Eigen::Vector3f& scaleFactor) const; + TriMeshModelPtr clone(const Eigen::Vector3f& scaleFactor) const; std::vector<Eigen::Vector3f> normals; std::vector<Eigen::Vector3f> vertices; diff --git a/VirtualRobot/Visualization/Visualization.cpp b/VirtualRobot/Visualization/Visualization.cpp index 77b523d54b676d6204d9dde77cb44ac7f4971fe8..544c5837ca98d96e60dc46f0d16b8ced70e7a481 100644 --- a/VirtualRobot/Visualization/Visualization.cpp +++ b/VirtualRobot/Visualization/Visualization.cpp @@ -24,8 +24,7 @@ namespace VirtualRobot } Visualization::~Visualization() - { - } + = default; VirtualRobot::VisualizationPtr Visualization::clone() { diff --git a/VirtualRobot/Workspace/Manipulability.cpp b/VirtualRobot/Workspace/Manipulability.cpp index e9499b6851e662203ffffcb274ddeac069b025e7..721129f19582ebad1701e4b2effbff7c6684a3ec 100644 --- a/VirtualRobot/Workspace/Manipulability.cpp +++ b/VirtualRobot/Workspace/Manipulability.cpp @@ -12,8 +12,8 @@ #include <fstream> #include <cmath> -#include <float.h> -#include <limits.h> +#include <cfloat> +#include <climits> #include <algorithm> #include <thread> diff --git a/VirtualRobot/Workspace/Reachability.cpp b/VirtualRobot/Workspace/Reachability.cpp index 8fc75ddeeb22a6ef184797dc130c224124832bc9..b93731fed815395be24d24b83e237ab678226f72 100644 --- a/VirtualRobot/Workspace/Reachability.cpp +++ b/VirtualRobot/Workspace/Reachability.cpp @@ -7,8 +7,8 @@ #include "../Grasping/GraspSet.h" #include <fstream> #include <cmath> -#include <float.h> -#include <limits.h> +#include <cfloat> +#include <climits> namespace VirtualRobot { diff --git a/VirtualRobot/Workspace/WorkspaceDataArray.cpp b/VirtualRobot/Workspace/WorkspaceDataArray.cpp index dad0ecbf3984be6e2917b83812cc649c72fb990a..5a888dc9ee56c1e280a57680385501bdaab87911 100644 --- a/VirtualRobot/Workspace/WorkspaceDataArray.cpp +++ b/VirtualRobot/Workspace/WorkspaceDataArray.cpp @@ -2,8 +2,8 @@ #include <fstream> #include <cmath> -#include <float.h> -#include <limits.h> +#include <cfloat> +#include <climits> namespace VirtualRobot { @@ -39,7 +39,7 @@ namespace VirtualRobot { for (unsigned int z = 0; z < size3; z++) { - data[x * sizeTr0 + y * sizeTr1 + z] = NULL; + data[x * sizeTr0 + y * sizeTr1 + z] = nullptr; } } } @@ -94,14 +94,14 @@ namespace VirtualRobot { int pos = x * sizeTr0 + y * sizeTr1 + z; - if (other->data[pos] != NULL) + if (other->data[pos] != nullptr) { data[pos] = new unsigned char[(unsigned int)sizeRot]; memcpy(data[pos], other->data[pos], (unsigned int)sizeRot * sizeof(unsigned char)); } else { - data[pos] = NULL; + data[pos] = nullptr; } } } @@ -497,7 +497,7 @@ namespace VirtualRobot if (data[x * sizeTr0 + y * sizeTr1 + z]) { delete [] data[x * sizeTr0 + y * sizeTr1 + z]; - data[x * sizeTr0 + y * sizeTr1 + z] = NULL; + data[x * sizeTr0 + y * sizeTr1 + z] = nullptr; } } } @@ -514,7 +514,7 @@ namespace VirtualRobot return false; } - return (data[x * sizeTr0 + y * sizeTr1 + z] != NULL); + return (data[x * sizeTr0 + y * sizeTr1 + z] != nullptr); } WorkspaceData* WorkspaceDataArray::clone() diff --git a/VirtualRobot/Workspace/WorkspaceGrid.cpp b/VirtualRobot/Workspace/WorkspaceGrid.cpp index 2b9c67d8ff90d2ee1a3ec670862c16b4165139ab..d322e388814f8394458725ae0ad5875262ce8b8c 100644 --- a/VirtualRobot/Workspace/WorkspaceGrid.cpp +++ b/VirtualRobot/Workspace/WorkspaceGrid.cpp @@ -21,7 +21,7 @@ namespace VirtualRobot minY = fMinY; maxY = fMaxY; discretizeSize = fDiscretizeSize; - data = NULL; + data = nullptr; if (fMinX >= fMaxX || fMinY >= fMaxY) { @@ -275,13 +275,13 @@ namespace VirtualRobot float x, y; - for (int i = 0; i < (int)wsData.size(); i++) + for (auto & i : wsData) { - Eigen::Matrix4f tmpPos2 = graspGlobal * wsData[i]->transformation.inverse(); + Eigen::Matrix4f tmpPos2 = graspGlobal * i->transformation.inverse(); x = tmpPos2(0, 3); y = tmpPos2(1, 3); - setEntryCheckNeighbors(x, y, wsData[i]->value, grasp); + setEntryCheckNeighbors(x, y, i->value, grasp); //setEntry(x,y,vData[i].value); } } diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp index cfb3b81b2f466cc430bf16448bc4b2a475175bf3..99f3d328a5588d9d677c5ac222f532b5569bcf27 100644 --- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp +++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp @@ -16,8 +16,8 @@ #include <VirtualRobot/Random.h> #include <fstream> #include <cmath> -#include <float.h> -#include <limits.h> +#include <cfloat> +#include <climits> #include <thread> namespace VirtualRobot @@ -251,16 +251,16 @@ namespace VirtualRobot // Check joint limits std::vector<RobotNodePtr> nodes = nodeSet->getAllRobotNodes(); - for (std::vector<RobotNodePtr>::iterator n = nodes.begin(); n != nodes.end(); n++) + for (auto & node : nodes) { float limits[2]; FileIO::readArray<float>(limits, 2, file); //limits[0] = (int)(FileIO::read<ioIntTypeRead>(file)); //limits[1] = (int)(FileIO::read<ioIntTypeRead>(file)); - if (fabs((*n)->getJointLimitLo() - limits[0]) > 0.01 || fabs((*n)->getJointLimitHi() - limits[1]) > 0.01) + if (fabs(node->getJointLimitLo() - limits[0]) > 0.01 || fabs(node->getJointLimitHi() - limits[1]) > 0.01) { - VR_WARNING << "Joint limit mismatch for " << (*n)->getName() << ", min: " << (*n)->getJointLimitLo() << " / " << limits[0] << ", max: " << (*n)->getJointLimitHi() << " / " << limits[1] << std::endl; + VR_WARNING << "Joint limit mismatch for " << node->getName() << ", min: " << node->getJointLimitLo() << " / " << limits[0] << ", max: " << node->getJointLimitHi() << " / " << limits[1] << std::endl; } } } @@ -308,9 +308,9 @@ namespace VirtualRobot discretizeStepTranslation = FileIO::read<float>(file); discretizeStepRotation = FileIO::read<float>(file); - for (int i = 0; i < 6; i++) + for (int & numVoxel : numVoxels) { - numVoxels[i] = (int)(FileIO::read<ioIntTypeRead>(file)); + numVoxel = (int)(FileIO::read<ioIntTypeRead>(file)); } //FileIO::readArray<int>(numVoxels, 6, file); @@ -529,10 +529,10 @@ namespace VirtualRobot const std::vector<RobotNodePtr> nodes = nodeSet->getAllRobotNodes(); FileIO::write<ioIntTypeWrite>(file, (ioIntTypeWrite)(nodes.size())); - for (std::vector<RobotNodePtr>::const_iterator n = nodes.begin(); n != nodes.end(); n++) + for (const auto & node : nodes) { - FileIO::write<float>(file, (*n)->getJointLimitLo()); - FileIO::write<float>(file, (*n)->getJointLimitHi()); + FileIO::write<float>(file, node->getJointLimitLo()); + FileIO::write<float>(file, node->getJointLimitHi()); } // TCP name @@ -576,9 +576,9 @@ namespace VirtualRobot // Number of voxels //FileIO::writeArray<ioIntTypeWrite>(file, numVoxels, 6); - for (int i = 0; i < 6; i++) + for (int & numVoxel : numVoxels) { - FileIO::write<ioIntTypeWrite>(file, (ioIntTypeWrite)(numVoxels[i])); + FileIO::write<ioIntTypeWrite>(file, (ioIntTypeWrite)numVoxel); } FileIO::write<ioIntTypeWrite>(file, (ioIntTypeWrite)(data->getVoxelFilledCount())); @@ -1056,34 +1056,34 @@ namespace VirtualRobot cout << type << " workspace extend (as defined on construction):" << endl; cout << "Min boundary (local): "; - for (int i = 0; i < 6; i++) + for (float minBound : minBounds) { - cout << minBounds[i] << ","; + cout << minBound << ","; } cout << endl; cout << "Max boundary (local): "; - for (int i = 0; i < 6; i++) + for (float maxBound : maxBounds) { - cout << maxBounds[i] << ","; + cout << maxBound << ","; } cout << endl; cout << "6D values achieved during buildup:" << endl; cout << "Minimum 6D values: "; - for (int i = 0; i < 6; i++) + for (float achievedMinValue : achievedMinValues) { - cout << achievedMinValues[i] << ","; + cout << achievedMinValue << ","; } cout << endl; cout << "Maximum 6D values: "; - for (int i = 0; i < 6; i++) + for (float achievedMaxValue : achievedMaxValues) { - cout << achievedMaxValues[i] << ","; + cout << achievedMaxValue << ","; } cout << endl; @@ -1397,9 +1397,9 @@ namespace VirtualRobot int sum = 0; int count = 0; - for (int i = 0; i < 6; i++) + for (unsigned int & i : x) { - x[i]--; + i--; if (isCovered(x)) { @@ -1407,8 +1407,8 @@ namespace VirtualRobot count++; } - x[i]++; - x[i]++; + i++; + i++; if (isCovered(x)) { @@ -1416,7 +1416,7 @@ namespace VirtualRobot count++; } - x[i]--; + i--; } if (count >= (int)minNeighbors) @@ -1768,18 +1768,18 @@ namespace VirtualRobot storeMinBBox = quadPos[0]; storeMaxBBox = quadPos[0]; - for (int k = 0; k < 8; k++) + for (auto & quadPo : quadPos) { for (int i = 0; i < 3; i++) { - if (quadPos[k](i) < storeMinBBox(i)) + if (quadPo(i) < storeMinBBox(i)) { - storeMinBBox(i) = quadPos[k](i); + storeMinBBox(i) = quadPo(i); } - if (quadPos[k](i) > storeMaxBBox(i)) + if (quadPo(i) > storeMaxBBox(i)) { - storeMaxBBox(i) = quadPos[k](i); + storeMaxBBox(i) = quadPo(i); } } } diff --git a/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp b/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp index dd9ca457ebfbba9104f51b50bedd85bd2680aaa0..559b140a905059b9c1ee0cfe7a4ac21042ff614f 100644 --- a/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp +++ b/VirtualRobot/Workspace/tests/VoxelTreeTest.cpp @@ -66,17 +66,17 @@ BOOST_AUTO_TEST_CASE(VoxelTreeEntriesTest) VirtualRobot::VoxelTree6D<unsigned char> v(minB, maxB, 20.0f, 1.0f); float pos[6]; - for (int i = 0; i < 6; i++) + for (float & po : pos) { - pos[i] = 0; + po = 0; } unsigned char* c = v.getEntry(pos); - bool isNull = (c == NULL); + bool isNull = (c == nullptr); BOOST_REQUIRE(isNull); v.setEntry(pos, 10); c = v.getEntry(pos); - isNull = (c == NULL); + isNull = (c == nullptr); BOOST_REQUIRE(!isNull); BOOST_REQUIRE_EQUAL(*c, 10); @@ -101,22 +101,22 @@ BOOST_AUTO_TEST_CASE(VoxelTreeNDEntriesTest) VirtualRobot::VoxelTreeND<unsigned char, N> v(minB, maxB, discr, true); float pos[N]; - for (unsigned int i = 0; i < N; i++) + for (float & po : pos) { - pos[i] = 0; + po = 0; } unsigned char* c = v.getEntry(pos); - bool isNull = (c == NULL); + bool isNull = (c == nullptr); BOOST_REQUIRE(isNull); v.setEntry(pos, 10); c = v.getEntry(pos); - isNull = (c == NULL); + isNull = (c == nullptr); BOOST_REQUIRE(!isNull); BOOST_REQUIRE_EQUAL(*c, 10); VirtualRobot::VoxelTreeNDElement<unsigned char, N>* e = v.getLeafElement(pos); - BOOST_REQUIRE(e != NULL); + BOOST_REQUIRE(e != nullptr); BOOST_REQUIRE(*(e->getEntry()) == 10); for (unsigned int i = 0; i < N; i++) @@ -149,17 +149,17 @@ BOOST_AUTO_TEST_CASE(VoxelTreeNDSaveLoad) for (unsigned int i = 0; i < TEST_LOOPS; i++) { - for (unsigned int j = 0; j < N; j++) + for (float & po : pos) { - pos[j] = float(rand() % 10000) / 10000.0f * 200.0f - 100.0f; + po = float(rand() % 10000) / 10000.0f * 200.0f - 100.0f; } v.setEntry(pos, rand() % 255); } - for (unsigned int i = 0; i < N; i++) + for (float & po : pos) { - pos[i] = 17.0f; + po = 17.0f; } v.setEntry(pos, 10); @@ -167,7 +167,7 @@ BOOST_AUTO_TEST_CASE(VoxelTreeNDSaveLoad) v.save("testVoxelTree.bin"); VirtualRobot::VoxelTreeND<unsigned char, N>* v2 = VirtualRobot::VoxelTreeND<unsigned char, N>::load("testVoxelTree.bin"); unsigned char* c = v2->getEntry(pos); - bool isNull = (c == NULL); + bool isNull = (c == nullptr); BOOST_REQUIRE(!isNull); BOOST_REQUIRE_EQUAL(*c, 10); @@ -194,14 +194,14 @@ BOOST_AUTO_TEST_CASE(VoxelTreeNDIterator) const int TEST_LOOPS = 10000; float pos[TEST_LOOPS][N]; - for (int i = 0; i < TEST_LOOPS; i++) + for (auto & po : pos) { - for (unsigned int j = 0; j < N; j++) + for (float & j : po) { - pos[i][j] = float(rand() % 10000) / 10000.0f * 200.0f - 100.0f; + j = float(rand() % 10000) / 10000.0f * 200.0f - 100.0f; } - v.setEntry(pos[i], rand() % 255); + v.setEntry(po, rand() % 255); } VirtualRobot::VoxelTreeND<unsigned char, N>::ElementIterator it; @@ -237,9 +237,9 @@ BOOST_AUTO_TEST_CASE(VoxelTreeNDMem) float pos[N]; - for (unsigned int j = 0; j < N; j++) + for (float & po : pos) { - pos[j] = float(rand() % 10000) / 10000.0f * 200.0f - 100.0f; + po = float(rand() % 10000) / 10000.0f * 200.0f - 100.0f; } v.setEntry(pos, rand() % 255); diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp index b6b389f622e0e2c8c6dcf639f89136571de62a85..a15989eb02d3e72d1a140b7bb88abf9ba4fe9d06 100644 --- a/VirtualRobot/XML/BaseIO.cpp +++ b/VirtualRobot/XML/BaseIO.cpp @@ -21,12 +21,10 @@ namespace VirtualRobot boost::mutex BaseIO::mutex; BaseIO::BaseIO() - { - } + = default; BaseIO::~BaseIO() - { - } + = default; bool BaseIO::isTrue(const char* s) { @@ -52,7 +50,7 @@ namespace VirtualRobot */ float BaseIO::convertToFloat(const char* s) { - THROW_VR_EXCEPTION_IF(NULL == s, "Passing Null string to convertToFloat()"); + THROW_VR_EXCEPTION_IF(nullptr == s, "Passing Null string to convertToFloat()"); std::stringstream floatStream; floatStream << std::string(s); float result; @@ -67,7 +65,7 @@ namespace VirtualRobot int BaseIO::convertToInt(const char* s) { - THROW_VR_EXCEPTION_IF(NULL == s, "Passing Null string to convertToInt()"); + THROW_VR_EXCEPTION_IF(nullptr == s, "Passing Null string to convertToInt()"); std::stringstream intStream; intStream << std::string(s); int result; @@ -167,7 +165,7 @@ namespace VirtualRobot return; } - rapidxml::xml_node<>* trXMLNode = NULL; + rapidxml::xml_node<>* trXMLNode = nullptr; std::string nodeName = getLowerCase(transformXMLNode->name()); if (nodeName == "transform") @@ -356,16 +354,16 @@ namespace VirtualRobot Units uAngle("rad"); Units uLength("mm"); - for (size_t i = 0; i < unitsAttr.size(); i++) + for (auto & i : unitsAttr) { - if (unitsAttr[i].isAngle()) + if (i.isAngle()) { - uAngle = unitsAttr[i]; + uAngle = i; } - if (unitsAttr[i].isLength()) + if (i.isLength()) { - uLength = unitsAttr[i]; + uLength = i; } } @@ -430,7 +428,7 @@ namespace VirtualRobot attr = node->first_attribute("unitsTime", 0, false); } - return (attr != NULL); + return (attr != nullptr); } void BaseIO::getAllAttributes(rapidxml::xml_node<char>* node, const std::string& attrString, std::vector<std::string>& storeValues) @@ -457,9 +455,9 @@ namespace VirtualRobot getAllAttributes(node, "unitsAngle", attrStr); getAllAttributes(node, "unitsTime", attrStr); - for (size_t i = 0; i < attrStr.size(); i++) + for (auto & i : attrStr) { - Units unitsAttribute(getLowerCase(attrStr[i].c_str())); + Units unitsAttribute(getLowerCase(i.c_str())); result.push_back(unitsAttribute); } @@ -572,7 +570,7 @@ namespace VirtualRobot */ std::string BaseIO::getLowerCase(const char* c) { - THROW_VR_EXCEPTION_IF(NULL == c, "Passing Null string to getLowerCase()"); + THROW_VR_EXCEPTION_IF(nullptr == c, "Passing Null string to getLowerCase()"); std::string res = c; getLowerCase(res); return res; @@ -1343,16 +1341,16 @@ namespace VirtualRobot Units uWeight("kg"); Units uLength("m"); - for (size_t i = 0; i < unitsAttr.size(); i++) + for (auto & i : unitsAttr) { - if (unitsAttr[i].isWeight()) + if (i.isWeight()) { - uWeight = unitsAttr[i]; + uWeight = i; } - if (unitsAttr[i].isLength()) + if (i.isLength()) { - uLength = unitsAttr[i]; + uLength = i; } } @@ -1737,11 +1735,11 @@ namespace VirtualRobot RobotPtr r; - for (size_t i = 0; i < robots.size(); i++) + for (auto & robot : robots) { - if (robots[i]->getType() == robotName) + if (robot->getType() == robotName) { - r = robots[i]; + r = robot; break; } } diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp index 058b311e2e40bc1329d638e09e6a1cb77c06b6d8..5cbbd9224f4e72f3bc5ebaadfe9b6fb214226283 100644 --- a/VirtualRobot/XML/ObjectIO.cpp +++ b/VirtualRobot/XML/ObjectIO.cpp @@ -20,12 +20,10 @@ namespace VirtualRobot ObjectIO::ObjectIO() - { - } + = default; ObjectIO::~ObjectIO() - { - } + = default; VirtualRobot::ObstaclePtr ObjectIO::loadObstacle(const std::string& xmlFile) { @@ -135,9 +133,9 @@ namespace VirtualRobot // create & register configs std::map< std::string, float > rc; - for (size_t i = 0; i < configDefinitions.size(); i++) + for (auto & configDefinition : configDefinitions) { - rc[ configDefinitions[i].name ] = configDefinitions[i].value; + rc[ configDefinition.name ] = configDefinition.value; } grasp->setConfiguration(rc); @@ -344,9 +342,9 @@ namespace VirtualRobot // build object ManipulationObjectPtr object(new ManipulationObject(objName, visualizationNode, collisionModel, physics)); - for (size_t i = 0; i < graspSets.size(); i++) + for (const auto & graspSet : graspSets) { - object->addGraspSet(graspSets[i]); + object->addGraspSet(graspSet); } object->setGlobalPose(globalPose); diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp index 6a1c80483cb2022264fd3604f223c9f42e93856d..825ce0244d87dab68ea615820f952ea5859be428 100644 --- a/VirtualRobot/XML/RobotIO.cpp +++ b/VirtualRobot/XML/RobotIO.cpp @@ -15,6 +15,7 @@ #include "../RobotConfig.h" #include "../RuntimeEnvironment.h" #include "rapidxml.hpp" +#include "mjcf/MujocoIO.h" #include <vector> @@ -28,12 +29,10 @@ namespace VirtualRobot std::map<std::string, int> RobotIO::robot_name_counter; RobotIO::RobotIO() - { - } + = default; RobotIO::~RobotIO() - { - } + = default; @@ -111,7 +110,7 @@ namespace VirtualRobot else { VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -180 [deg]." << endl; - jointLimitLo = (float)(-M_PI); + jointLimitLo = float(-M_PI); unit = Units("rad"); } } @@ -131,7 +130,7 @@ namespace VirtualRobot else { VR_WARNING << "No 'hi' attribute in <Limits> tag. Assuming 180 [deg]." << endl; - jointLimitHi = (float)M_PI; + jointLimitHi = float(M_PI); unit = Units("rad"); } } @@ -153,15 +152,15 @@ namespace VirtualRobot // limitless attribute rapidxml::xml_attribute<>* llAttr = limitsXMLNode->first_attribute("limitless"); - if (llAttr != NULL) + if (llAttr != nullptr) { limitless = isTrue(llAttr->value()); if (limitless && unit.isAngle() && (unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360)) { VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes should equal 2*pi [rad] or 360 [deg]." << endl << "Setting 'lo' to -pi and 'hi' to pi [rad]..." << endl; - jointLimitLo = -M_PI; - jointLimitHi = M_PI; + jointLimitLo = float(-M_PI); + jointLimitHi = float(M_PI); } } @@ -193,7 +192,7 @@ namespace VirtualRobot SensorPtr s; - SensorFactoryPtr sensorFactory = SensorFactory::fromName(sensorType, NULL); + SensorFactoryPtr sensorFactory = SensorFactory::fromName(sensorType, nullptr); if (sensorFactory) { @@ -217,8 +216,8 @@ namespace VirtualRobot Eigen::Matrix4f& transformationMatrix ) { - float jointLimitLow = (float) - M_PI; - float jointLimitHigh = (float)M_PI; + float jointLimitLow = float(- M_PI); + float jointLimitHigh = float(M_PI); bool limitless = false; Eigen::Matrix4f preJointTransform = transformationMatrix;//Eigen::Matrix4f::Identity(); @@ -238,7 +237,7 @@ namespace VirtualRobot if (!jointXMLNode) { // no <Joint> tag -> fixed joint - RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), NULL); + RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr); if (fixedNodeFactory) { @@ -275,9 +274,9 @@ namespace VirtualRobot } rapidxml::xml_node<>* node = jointXMLNode->first_node(); - rapidxml::xml_node<>* tmpXMLNodeAxis = NULL; - rapidxml::xml_node<>* tmpXMLNodeTranslation = NULL; - rapidxml::xml_node<>* limitsNode = NULL; + rapidxml::xml_node<>* tmpXMLNodeAxis = nullptr; + rapidxml::xml_node<>* tmpXMLNodeTranslation = nullptr; + rapidxml::xml_node<>* limitsNode = nullptr; float maxVelocity = -1.0f; // m/s float maxAcceleration = -1.0f; // m/s^2 @@ -334,21 +333,21 @@ namespace VirtualRobot Units uLength("m"); Units uAngle("rad"); - for (size_t i = 0; i < unitsAttr.size(); i++) + for (auto & i : unitsAttr) { - if (unitsAttr[i].isTime()) + if (i.isTime()) { - uTime = unitsAttr[i]; + uTime = i; } - if (unitsAttr[i].isLength()) + if (i.isLength()) { - uLength = unitsAttr[i]; + uLength = i; } - if (unitsAttr[i].isAngle()) + if (i.isAngle()) { - uAngle = unitsAttr[i]; + uAngle = i; } } @@ -371,7 +370,7 @@ namespace VirtualRobot if (uAngle.isDegree()) { - factor *= float(M_PI / 180.0f); + factor *= float(M_PI / 180.0); } maxVelocity *= factor; @@ -387,21 +386,21 @@ namespace VirtualRobot Units uLength("m"); Units uAngle("rad"); - for (size_t i = 0; i < unitsAttr.size(); i++) + for (auto & i : unitsAttr) { - if (unitsAttr[i].isTime()) + if (i.isTime()) { - uTime = unitsAttr[i]; + uTime = i; } - if (unitsAttr[i].isLength()) + if (i.isLength()) { - uLength = unitsAttr[i]; + uLength = i; } - if (unitsAttr[i].isAngle()) + if (i.isAngle()) { - uAngle = unitsAttr[i]; + uAngle = i; } } @@ -424,7 +423,7 @@ namespace VirtualRobot if (uAngle.isDegree()) { - factor *= float(M_PI / 180.0f); + factor *= float(M_PI / 180.0); } maxAcceleration *= factor; @@ -437,11 +436,11 @@ namespace VirtualRobot std::vector< Units > unitsAttr = getUnitsAttributes(node); Units uLength("m"); - for (size_t i = 0; i < unitsAttr.size(); i++) + for (auto & i : unitsAttr) { - if (unitsAttr[i].isLength()) + if (i.isLength()) { - uLength = unitsAttr[i]; + uLength = i; } } @@ -544,7 +543,7 @@ namespace VirtualRobot //} - RobotNodeFactoryPtr robotNodeFactory = RobotNodeFactory::fromName(jointType, NULL); + RobotNodeFactoryPtr robotNodeFactory = RobotNodeFactory::fromName(jointType, nullptr); if (robotNodeFactory) { @@ -644,7 +643,7 @@ namespace VirtualRobot Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity(); rapidxml::xml_node<>* node = robotNodeXMLNode->first_node(); - rapidxml::xml_node<>* jointNodeXML = NULL; + rapidxml::xml_node<>* jointNodeXML = nullptr; std::vector< rapidxml::xml_node<>* > sensorTags; @@ -737,9 +736,9 @@ namespace VirtualRobot robotNode = processJointNode(jointNodeXML, robotNodeName, robo, visualizationNode, collisionModel, physics, rntype, transformMatrix); // process sensors - for (size_t i = 0; i < sensorTags.size(); i++) + for (auto & sensorTag : sensorTags) { - processSensor(robotNode, sensorTags[i], loadMode, basePath); + processSensor(robotNode, sensorTag, loadMode, basePath); } return robotNode; @@ -787,7 +786,7 @@ namespace VirtualRobot if (useColModel && visuFile != "") { - VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuFileType, NULL); + VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuFileType, nullptr); if (visualizationFactory) { @@ -827,9 +826,9 @@ namespace VirtualRobot std::vector< ChildFromRobotDef > childrenFromRobot = iter->second; RobotNodePtr node = iter->first; - for (unsigned int i = 0; i < childrenFromRobot.size(); i++) + for (auto & i : childrenFromRobot) { - boost::filesystem::path filenameNew(childrenFromRobot[i].filename); + boost::filesystem::path filenameNew(i.filename); boost::filesystem::path filenameBasePath(basePath); boost::filesystem::path filenameNewComplete = boost::filesystem::operator/(filenameBasePath, filenameNew); @@ -845,27 +844,27 @@ namespace VirtualRobot } RobotPtr r = loadRobot(filenameNewComplete.string(), loadMode); - THROW_VR_EXCEPTION_IF(!r, "Could not add child-from-robot due to failed loading of robot from file" << childrenFromRobot[i].filename); + THROW_VR_EXCEPTION_IF(!r, "Could not add child-from-robot due to failed loading of robot from file" << i.filename); RobotNodePtr root = r->getRootNode(); - THROW_VR_EXCEPTION_IF(!root, "Could not add child-from-robot. No root node in file" << childrenFromRobot[i].filename); + THROW_VR_EXCEPTION_IF(!root, "Could not add child-from-robot. No root node in file" << i.filename); RobotNodePtr rootNew = root->clone(robo, true, node); - THROW_VR_EXCEPTION_IF(!rootNew, "Clone failed. Could not add child-from-robot from file " << childrenFromRobot[i].filename); + THROW_VR_EXCEPTION_IF(!rootNew, "Clone failed. Could not add child-from-robot from file " << i.filename); std::vector<EndEffectorPtr> eefs; r->getEndEffectors(eefs); - for (std::vector<EndEffectorPtr>::iterator eef = eefs.begin(); eef != eefs.end(); eef++) + for (auto & eef : eefs) { - (*eef)->clone(robo); + eef->clone(robo); } std::vector<RobotNodeSetPtr> nodeSets; r->getRobotNodeSets(nodeSets); - for (std::vector<RobotNodeSetPtr>::iterator ns = nodeSets.begin(); ns != nodeSets.end(); ns++) + for (auto & nodeSet : nodeSets) { - (*ns)->clone(robo); + nodeSet->clone(robo); } // already performed in root->clone @@ -876,17 +875,17 @@ namespace VirtualRobot } //std::vector<RobotNodeSetPtr> robotNodeSets - for (unsigned int i = 0; i < endeffectorNodes.size(); ++i) + for (auto & endeffectorNode : endeffectorNodes) { - EndEffectorPtr eef = processEndeffectorNode(endeffectorNodes[i], robo); + EndEffectorPtr eef = processEndeffectorNode(endeffectorNode, robo); robo->registerEndEffector(eef); } int rnsCounter = 0; - for (int i = 0; i < (int)robotNodeSetNodes.size(); ++i) + for (auto & robotNodeSetNode : robotNodeSetNodes) { - RobotNodeSetPtr rns = processRobotNodeSet(robotNodeSetNodes[i], robo, robotRoot, rnsCounter); + RobotNodeSetPtr rns = processRobotNodeSet(robotNodeSetNode, robo, robotRoot, rnsCounter); //nodeSets.push_back(rns); } @@ -894,11 +893,11 @@ namespace VirtualRobot robo->getRobotNodes(nodes); RobotNodePtr root = robo->getRootNode(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - if (nodes[i] != root && !(nodes[i]->getParent())) + if (node != root && !(node->getParent())) { - THROW_VR_EXCEPTION("Node without parent: " << nodes[i]->getName()); + THROW_VR_EXCEPTION("Node without parent: " << node->getName()); } } @@ -998,7 +997,7 @@ namespace VirtualRobot //std::vector<rapidxml::xml_node<>* > robotNodeSetNodes; //std::vector<rapidxml::xml_node<>* > endeffectorNodes; - rapidxml::xml_node<>* XMLNode = robotXMLNode->first_node(NULL, 0, false); + rapidxml::xml_node<>* XMLNode = robotXMLNode->first_node(nullptr, 0, false); while (XMLNode) { @@ -1037,9 +1036,9 @@ namespace VirtualRobot else { // double name check - for (unsigned int i = 0; i < robotNodes.size(); i++) + for (auto & robotNode : robotNodes) { - THROW_VR_EXCEPTION_IF((robotNodes[i]->getName() == n->getName()), "At least two RobotNodes with name <" << n->getName() << "> defined in robot definition"); + THROW_VR_EXCEPTION_IF((robotNode->getName() == n->getName()), "At least two RobotNodes with name <" << n->getName() << "> defined in robot definition"); } childrenMap[n] = childrenNames; @@ -1076,7 +1075,7 @@ namespace VirtualRobot THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl); } - XMLNode = XMLNode->next_sibling(NULL, 0, false); + XMLNode = XMLNode->next_sibling(nullptr, 0, false); } THROW_VR_EXCEPTION_IF(robotNodes.empty(), "No RobotNodes defined in Robot."); @@ -1505,14 +1504,22 @@ namespace VirtualRobot { std::vector<RobotNodePtr> nodes = robot->getRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - nodes[i]->saveModelFiles(modelDirComplete.string(), true); + node->saveModelFiles(modelDirComplete.string(), true); } } return true; } + + void RobotIO::saveMJCF(RobotPtr robot, const std::string& filename, + const std::string& basePath, const std::string& meshDir) + { + mjcf::MujocoIO mujocoIO(robot); + mujocoIO.setActuatorType(mjcf::ActuatorType::MOTOR); + mujocoIO.saveMJCF(filename, basePath, meshDir); + } } // namespace VirtualRobot diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h index e9dd02e86febdff633291ce283bae354bf3ad1d0..7db20aab05f0b84091ee183260f872fcab714bc7 100644 --- a/VirtualRobot/XML/RobotIO.h +++ b/VirtualRobot/XML/RobotIO.h @@ -58,19 +58,19 @@ namespace VirtualRobot }; /*! - Loads robot from file. - @param xmlFile The file - @param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access. - @return Returns an empty pointer, when file access failed. + Loads robot from file. + @param xmlFile The file + @param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access. + @return Returns an empty pointer, when file access failed. */ static RobotPtr loadRobot(const std::string& xmlFile, RobotDescription loadMode = eFull); /*! - Creates Robot from string. - @param xmlString The input string. - @param basePath If any \<childFromRobot\> tags are given, the path for searching the robot files can be specified. - @param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access. - */ + Creates Robot from string. + @param xmlString The input string. + @param basePath If any \<childFromRobot\> tags are given, the path for searching the robot files can be specified. + @param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access. + */ static RobotPtr createRobotFromString(const std::string& xmlString, const std::string& basePath = "", RobotDescription loadMode = eFull); @@ -84,6 +84,18 @@ namespace VirtualRobot static bool saveXML(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& modelDir = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true, bool storeModelFiles = true); + /*! + @brief saveMJCF + @param robot The robot to save. + @param filename The filename without path. + @param basePath The directory to store the robot to + @param meshDir The local directory where all mesh files should be stored to. + */ + static void saveMJCF(RobotPtr robot, + const std::string& filename, const std::string& basePath, + const std::string& meshDir = "mesh"); + + protected: struct ChildFromRobotDef diff --git a/VirtualRobot/XML/SceneIO.cpp b/VirtualRobot/XML/SceneIO.cpp index d573bf1c45d0d347a68039ef6bc7086910c9b026..bf3af0dcb88113048a04b69dde850f26746db954 100644 --- a/VirtualRobot/XML/SceneIO.cpp +++ b/VirtualRobot/XML/SceneIO.cpp @@ -14,12 +14,10 @@ namespace VirtualRobot SceneIO::SceneIO() - { - } + = default; SceneIO::~SceneIO() - { - } + = default; VirtualRobot::ScenePtr SceneIO::loadScene(const std::string& xmlFile) { @@ -123,10 +121,10 @@ namespace VirtualRobot // create & register node sets int rnsNr = 0; - for (size_t i = 0; i < rnsNodes.size(); i++) + for (auto & rnsNode : rnsNodes) { // registers rns to robot - RobotNodeSetPtr r = processRobotNodeSet(rnsNodes[i], robot, robot->getRootNode()->getName(), rnsNr); + RobotNodeSetPtr r = processRobotNodeSet(rnsNode, robot, robot->getRootNode()->getName(), rnsNr); THROW_VR_EXCEPTION_IF(!r, "Invalid RobotNodeSet definition " << endl); } @@ -260,7 +258,7 @@ namespace VirtualRobot std::vector<rapidxml::xml_node<char>* > trajectoryNodes; - rapidxml::xml_node<>* XMLNode = sceneXMLNode->first_node(NULL, 0, false); + rapidxml::xml_node<>* XMLNode = sceneXMLNode->first_node(nullptr, 0, false); while (XMLNode) { @@ -312,13 +310,13 @@ namespace VirtualRobot THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl); } - XMLNode = XMLNode->next_sibling(NULL, 0, false); + XMLNode = XMLNode->next_sibling(nullptr, 0, false); } // process all sceneSetNodes - for (size_t i = 0; i < sceneSetNodes.size(); i++) + for (auto & sceneSetNode : sceneSetNodes) { - bool r = processSceneObjectSet(sceneSetNodes[i], scene); + bool r = processSceneObjectSet(sceneSetNode, scene); if (!r) { @@ -329,9 +327,9 @@ namespace VirtualRobot // process all trajectories - for (size_t i = 0; i < trajectoryNodes.size(); i++) + for (auto & trajectoryNode : trajectoryNodes) { - bool r = processSceneTrajectory(trajectoryNodes[i], scene); + bool r = processSceneTrajectory(trajectoryNode, scene); if (!r) { diff --git a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e04389f540d4d08a5e3019ba8b96a57d8e5e0926 --- /dev/null +++ b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp @@ -0,0 +1,263 @@ +#include "MasslessBodySanitizer.h" +#include "utils.h" + +#include <boost/algorithm/string/join.hpp> + + +using namespace VirtualRobot; +using namespace mjcf; +namespace tx = tinyxml2; + + +MasslessBodySanitizer::MasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot) : + document(document), robot(robot) +{ + +} + +void MasslessBodySanitizer::sanitize() +{ + // merge body leaf nodes with parent if they do not have a mass (inertial or geom) + + XMLElement* root = document->robotRootBody(); + + for (XMLElement* body = root->FirstChildElement("body"); + body; + body = body->NextSiblingElement("body")) + { + sanitizeRecursion(body); + } +} + + +void MasslessBodySanitizer::sanitizeRecursion(XMLElement* body) +{ + assertElementIsBody(body); + + std::string bodyName = body->Attribute("name"); + RobotNodePtr bodyNode = robot->getRobotNode(bodyName); + Eigen::Matrix4f accChildPose = Eigen::Matrix4f::Identity(); + + while (!hasMass(body)) + { + std::cout << t << bodyName << ": \t"; + + if (!hasElementChild(body, "body")) + { + // leaf => end of recursion + sanitizeLeafBody(body); + return; + } + + // non-leaf body + // check whether there is only one child body + XMLElement* childBody = body->FirstChildElement("body"); + if (!childBody->NextSiblingElement("body")) + { + mergeBodies(body, childBody, accChildPose); + } + else + { + std::cout << "Adding dummy inertial to massless body with >1 child bodies." << std::endl; + // add a small mass + document->addDummyInertial(body, true); + break; + } + } + + + // recursive descend + + for (XMLElement* child = body->FirstChildElement("body"); + child; + child = child->NextSiblingElement("body")) + { + sanitizeRecursion(child); + } + +} + +void MasslessBodySanitizer::mergeBodies(XMLElement* body, XMLElement* childBody, + Eigen::Matrix4f& accChildPose) +{ + std::string childBodyName = childBody->Attribute("name"); + + std::cout << "Merging with '" << childBodyName << "' "; + + RobotNodePtr childNode = robot->getRobotNode(childBodyName); + Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent()); + + // update accumulated child pose + // merged child's frame w.r.t. body's frame + accChildPose = childPose * accChildPose; + Eigen::Matrix3f accChildOri = accChildPose.block<3,3>(0, 0); + + // merge childBody into body => move all its elements here + // while doing this, apply accChildPose to elements + for (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild; + grandChild = grandChild->NextSibling()) + { + // clone grandchild + tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr); + + XMLElement* elem = grandChildClone->ToElement(); + if (elem) + { + /* Adapt pose/axis elements in child. Their poses/axes will be + * relative to body's frame, so the transformation from body + * to child will be lost. Therefore, apply accChildPose to + * their poses/axes. */ + + if (isElement(elem, "joint")) + { + // update pos and axis + updateChildPos(elem, accChildPose); + updateChildAxis(elem, accChildOri); + } + else if (isElement(elem, "body") + || isElement(elem, "inertial") + || isElement(elem, "geom") + || isElement(elem, "site") + || isElement(elem, "camera")) + { + updateChildPos(elem, accChildPose); + updateChildQuat(elem, accChildOri); + } + else if (isElement(elem, "light")) + { + updateChildPos(elem, accChildPose); + updateChildAxis(elem, accChildOri, "dir"); + } + } + + // insert to body + body->InsertEndChild(grandChildClone); + } + + // update body name + MergedBodySet& bodySet = getMergedBodySetWith(body->Attribute("name")); + bodySet.addBody(childBodyName); + body->SetAttribute("name", bodySet.getMergedBodyName().c_str()); + + std::cout << "\t(new name: " << bodySet.getMergedBodyName() << ")" << std::endl; + + // delete child + body->DeleteChild(childBody); +} + +void MasslessBodySanitizer::updateChildPos(XMLElement* elem, const Eigen::Matrix4f& accChildPose) +{ + const char* posStr = elem->Attribute("pos"); + Eigen::Vector3f pos = posStr ? strToVec(posStr) : + Eigen::Vector3f::Zero(); + + Eigen::Vector4f posHom; + posHom << pos, 1; + posHom = accChildPose * posHom; + pos = posHom.head<3>(); + + document->setElementPos(elem, pos); +} + +void MasslessBodySanitizer::updateChildQuat(XMLElement* elem, const Eigen::Matrix3f& accChildOri) +{ + const char* quatStr = elem->Attribute("quat"); + Eigen::Quaternionf quat = quatStr ? strToQuat(quatStr) : + Eigen::Quaternionf::Identity(); + + quat = accChildOri * quat; + document->setElementQuat(elem, quat); +} + +void MasslessBodySanitizer::updateChildAxis(XMLElement* elem, const Eigen::Matrix3f& accChildOri, + const char* attrName) +{ + Eigen::Vector3f axis = strToVec(elem->Attribute(attrName)); + axis = accChildOri * axis; + elem->SetAttribute(attrName, toAttr(axis).c_str()); + + if (strcmp(attrName, "axis") == 0) + { + document->setJointAxis(elem, axis); + } + else + { + elem->SetAttribute(attrName, toAttr(axis).c_str()); + } +} + +void MasslessBodySanitizer::sanitizeLeafBody(XMLElement* body) +{ + assert(!hasElementChild(body, "body")); + assert(!hasMass(body)); + + if (body->NoChildren()) // is completely empty? + { + // leaf without geom: make it a site + std::cout << "Changing to site." << std::endl; + body->SetValue("site"); + } + else + { + // add a small mass + std::cout << "Adding dummy inertial to massless leaf body with children." << std::endl; + document->addDummyInertial(body); + } +} + +const std::vector<MergedBodySet>& MasslessBodySanitizer::getMergedBodySets() const +{ + return mergedBodySets; +} + +const std::string& MasslessBodySanitizer::getMergedBodyName(const std::string& originalBodyName) +{ + return getMergedBodySetWith(originalBodyName).getMergedBodyName(); +} + +MergedBodySet&MasslessBodySanitizer::getMergedBodySetWith(const std::string& bodyName) +{ + for (auto& set : mergedBodySets) + { + if (set.containsBody(bodyName)) + { + return set; + } + } + + // not found => add + mergedBodySets.push_back(MergedBodySet(bodyName)); + + return mergedBodySets.back(); +} + + +MergedBodySet::MergedBodySet() += default; + +MergedBodySet::MergedBodySet(const std::string& bodyName) +{ + addBody(bodyName); +} + +void MergedBodySet::addBody(const std::string& bodyName) +{ + originalBodyNames.push_back(bodyName); + updateMergedBodyName(); +} + +bool MergedBodySet::containsBody(const std::string& bodyName) const +{ + return std::find(originalBodyNames.begin(), originalBodyNames.end(), + bodyName) != originalBodyNames.end(); +} + +const std::string& MergedBodySet::getMergedBodyName() const +{ + return mergedBodyName; +} + +void MergedBodySet::updateMergedBodyName() +{ + mergedBodyName = boost::algorithm::join(originalBodyNames, "~"); +} diff --git a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.h b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.h new file mode 100644 index 0000000000000000000000000000000000000000..d3bef857d759621187c48c273d88a313078514a0 --- /dev/null +++ b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.h @@ -0,0 +1,74 @@ +#pragma once + +#include "MjcfDocument.h" + + +namespace VirtualRobot +{ +namespace mjcf +{ + + class MergedBodySet + { + public: + + MergedBodySet(); + MergedBodySet(const std::string& bodyName); + + void addBody(const std::string& bodyName); + bool containsBody(const std::string& bodyName) const; + + const std::string& getMergedBodyName() const; + + + private: + + void updateMergedBodyName(); + + std::string mergedBodyName; + std::vector<std::string> originalBodyNames; + + }; + + + class MasslessBodySanitizer + { + public: + + MasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot); + + void sanitize(); + + const std::vector<MergedBodySet>& getMergedBodySets() const; + + const std::string& getMergedBodyName(const std::string& originalBodyName); + + MergedBodySet& getMergedBodySetWith(const std::string& bodyName); + + + private: + + void sanitizeRecursion(mjcf::XMLElement* body); + void sanitizeLeafBody(mjcf::XMLElement* body); + + void mergeBodies(XMLElement* body, XMLElement* childBody, Eigen::Matrix4f& accChildPose); + + void updateChildPos(XMLElement* elem, const Eigen::Matrix4f& accChildPose); + void updateChildQuat(XMLElement* elem, const Eigen::Matrix3f& accChildOri); + void updateChildAxis(XMLElement* elem, const Eigen::Matrix3f& accChildOri, + const char* attrName = "axis"); + + + + const std::string t = "| "; + + + DocumentPtr& document; + RobotPtr& robot; + + std::vector<MergedBodySet> mergedBodySets; + + }; + +} +} diff --git a/VirtualRobot/XML/mjcf/MjcfDocument.cpp b/VirtualRobot/XML/mjcf/MjcfDocument.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0224841e54cda8a1c3c464a1d0d36c0dc1027a54 --- /dev/null +++ b/VirtualRobot/XML/mjcf/MjcfDocument.cpp @@ -0,0 +1,425 @@ +#include "MjcfDocument.h" + +#include <VirtualRobot/Nodes/RobotNodePrismatic.h> +#include <VirtualRobot/Nodes/RobotNodeRevolute.h> + + +using namespace VirtualRobot; +using namespace mjcf; + + +Document::Document() +{ + // create root element + root = NewElement("mujoco"); + this->InsertEndChild(root); +} + +void Document::setModelName(const std::string& name) +{ + root->SetAttribute("model", name.c_str()); +} + +void Document::setNewElementClass(const std::string& className, bool excludeBody) +{ + this->newElementClass = className; + this->newElementClassExcludeBody = excludeBody; +} + +XMLElement* Document::addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2) +{ + XMLElement* texSkybox = addNewElement(asset(), "texture"); + + texSkybox->SetAttribute("type", "skybox"); + texSkybox->SetAttribute("builtin", "gradient"); + texSkybox->SetAttribute("width", 128); + texSkybox->SetAttribute("height", 128); + texSkybox->SetAttribute("rgb1", toAttr(rgb1).c_str()); + texSkybox->SetAttribute("rgb2", toAttr(rgb2).c_str()); + + return texSkybox; +} + +XMLElement* Document::addMocapBody(const std::string& name, float geomSize) +{ + /* + <body name="hand-ctrl" mocap="true" > + <geom type="box" size="0.01 0.01 0.01" contype="0" conaffinity="0" rgba="0.9 0.5 0.5 0.5" /> + </body> + */ + XMLElement* mocap = addNewElement(worldbody(), "body"); + mocap->SetAttribute("name", name.c_str()); + mocap->SetAttribute("mocap", "true"); + + // add geom for visualization + + XMLElement* geom = addNewElement(mocap, "geom"); + geom->SetAttribute("type", "box"); + setAttr(geom, "size", Eigen::Vector3f{geomSize, geomSize, geomSize}); + + return mocap; +} + + + +XMLElement* Document::addDefaultsClass(const std::string& className) +{ + XMLElement* def = addNewElement(default_(), "default", className); + + return def; +} + +XMLElement* Document::addRobotRootBodyElement(const std::string& robotName) +{ + this->robotRootBody_ = addNewElement(worldbody(), "body"); + + robotRootBody_->SetAttribute("name", robotName.c_str()); + robotRootBody_->SetAttribute("childclass", robotName.c_str()); + + return robotRootBody_; +} + +XMLElement* Document::addBodyElement(XMLElement* parent, RobotNodePtr node) +{ + XMLElement* body = addNewElement(parent, "body"); + body->SetAttribute("name", node->getName().c_str()); + + if (node->hasParent()) + { + Eigen::Matrix4f tf = node->getTransformationFrom(node->getParent()); + setElementPose(body, tf); + } + + if (node->isRotationalJoint() || node->isTranslationalJoint()) + { + addJointElement(body, node); + } + + addInertialElement(body, node); + + return body; +} + + +XMLElement* Document::addGeomElement(XMLElement* body, const std::string& meshName) +{ + assertElementIsBody(body); + + XMLElement* geom = addNewElement(body, "geom", "", true); + + geom->SetAttribute("type", "mesh"); + geom->SetAttribute("mesh", meshName.c_str()); + + return geom; +} + +XMLElement* Document::addInertialElement(XMLElement* body, RobotNodePtr node) +{ + assertElementIsBody(body); + + Eigen::Matrix3f matrix = node->getInertiaMatrix(); + if (matrix.isIdentity(floatCompPrecision) && node->getMass() < floatCompPrecision) + { + // dont set an inertial element and let it be derived automatically + return nullptr; + } + + XMLElement* inertial = addNewElement(body, "inertial"); + + Eigen::Vector3f pos = node->getCoMLocal(); + inertial->SetAttribute("pos", toAttr(pos).c_str()); + inertial->SetAttribute("mass", double(node->getMass())); + + if (matrix.isDiagonal(floatCompPrecision)) + { + Eigen::Vector3f diag = matrix.diagonal(); + inertial->SetAttribute("diaginertia", toAttr(diag).c_str()); + } + else + { + /* from mujoco xml reference: + * "Full inertia matrix M. Since M is 3-by-3 and symmetric, it is + * specified using only 6 numbers in the following order: + * M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3)." */ + Eigen::Matrix<float, 6, 1> inertia; + inertia << matrix(0, 0), matrix(1, 1), matrix(2, 2), + matrix(0, 1), matrix(0, 2), matrix(1, 2); + inertial->SetAttribute("fullinertia", toAttr(inertia).c_str()); + } + + return inertial; +} + +XMLElement*Document::addDummyInertial(XMLElement* body, bool first) +{ + assertElementIsBody(body); + + XMLElement* inertial = addNewElement(body, "inertial", "", first); + + inertial->SetAttribute("pos", toAttr(Eigen::Vector3f(0, 0, 0)).c_str()); + inertial->SetAttribute("mass", dummyMass); + inertial->SetAttribute("diaginertia", toAttr(Eigen::Vector3f(1, 1, 1)).c_str()); + + return inertial; +} + + +XMLElement* Document::addJointElement(XMLElement* body, RobotNodePtr node) +{ + assert(node->isRotationalJoint() xor node->isTranslationalJoint()); + + XMLElement* joint = addNewElement(body, "joint"); + + joint->SetAttribute("name", node->getName().c_str()); + + // get the axis + Eigen::Vector3f axis; + if (node->isRotationalJoint()) + { + RobotNodeRevolutePtr revolute = boost::dynamic_pointer_cast<RobotNodeRevolute>(node); + assert(revolute); + axis = revolute->getJointRotationAxisInJointCoordSystem(); + } + else if (node->isTranslationalJoint()) + { + RobotNodePrismaticPtr prismatic = boost::dynamic_pointer_cast<RobotNodePrismatic>(node); + assert(prismatic); + axis = prismatic->getJointTranslationDirectionJointCoordSystem(); + } + + joint->SetAttribute("type", node->isRotationalJoint() ? "hinge" : "slide"); + setJointAxis(joint, axis); + joint->SetAttribute("limited", toAttr(!node->isLimitless()).c_str()); + + if (!node->isLimitless()) + { + Eigen::Vector2f range(node->getJointLimitLow(), node->getJointLimitHigh()); + joint->SetAttribute("range", toAttr(range).c_str()); + } + + return joint; +} + +XMLElement* Document::addFreeJointElement(XMLElement* body) +{ + assertElementIsBody(body); + return addNewElement(body, "freejoint"); +} + +XMLElement* Document::addMeshElement(const std::string& name, const std::string& file, const std::string& className) +{ + XMLElement* mesh = addNewElement(asset(), "mesh", className); + + mesh->SetAttribute("name", name.c_str()); + mesh->SetAttribute("file", file.c_str()); + + return mesh; +} + +XMLElement* Document::addActuatorMotorElement(const std::string& jointName) +{ + return addActuatorShortcut("motor", jointName, jointName); +} + +XMLElement*Document::addActuatorPositionElement(const std::string& jointName, float kp) +{ + return addActuatorShortcut("position", jointName, jointName, "kp", kp); +} + +XMLElement*Document::addActuatorPositionElement(const std::string& jointName, bool ctrlLimited, + const Eigen::Vector2f& ctrlRange, float kp) +{ + XMLElement* act = addActuatorPositionElement(jointName, kp); + act->SetAttribute("ctrllimited", toAttr(ctrlLimited).c_str()); + act->SetAttribute("ctrlrange", toAttr(ctrlRange).c_str()); + return act; +} + +XMLElement*Document::addActuatorVelocityElement(const std::string& jointName, float kv) +{ + return addActuatorShortcut("velocity", jointName, jointName, "kv", kv); +} + +XMLElement* Document::addEqualityWeld(const std::string& name, const std::string& body1, const std::string& body2, const std::string& className) +{ + XMLElement* weld = addNewElement(equality(), "weld", className); + + weld->SetAttribute("name", name.c_str()); + weld->SetAttribute("body1", body1.c_str()); + weld->SetAttribute("body2", body2.c_str()); + + return weld; +} + + +XMLElement* Document::addActuatorShortcut( + const std::string& type, const std::string& name, const std::string& jointName, + const std::string& paramName, float paramValue) +{ + XMLElement* motor = addNewElement(actuator(), type.c_str()); + + motor->SetAttribute("name", name.c_str()); + motor->SetAttribute("joint", jointName.c_str()); + + if (!paramName.empty() && paramValue > floatCompPrecision) + { + motor->SetAttribute(paramName.c_str(), paramValue); + } + + return motor; +} + + +void Document::setElementPose(XMLElement* elem, const Eigen::Matrix4f& pose) +{ + Eigen::Vector3f pos = pose.block<3,1>(0, 3); + Eigen::Matrix3f oriMat = pose.block<3,3>(0, 0); + + Eigen::Quaternionf quat(oriMat); + + setElementPos(elem, pos); + setElementQuat(elem, quat); +} + +void Document::setElementPos(XMLElement* elem, Eigen::Vector3f pos) +{ + if (!pos.isZero(floatCompPrecision)) + { + elem->SetAttribute("pos", toAttr(pos).c_str()); + } + else + { + elem->DeleteAttribute("pos"); + } +} + +void Document::setElementQuat(XMLElement* elem, const Eigen::Quaternionf& quat) +{ + if (!quat.isApprox(Eigen::Quaternionf::Identity(), floatCompPrecision)) + { + elem->SetAttribute("quat", toAttr(quat).c_str()); + } + else + { + elem->DeleteAttribute("quat"); + } +} + +void Document::setJointAxis(XMLElement* joint, const Eigen::Vector3f& axis) +{ + assertElementIs(joint, "joint"); + joint->SetAttribute("axis", toAttr(axis).c_str()); +} + + +XMLElement* Document::addContactExclude(const XMLElement& body1, const XMLElement& body2) +{ + return addContactExclude(body1.Attribute("name"), body2.Attribute("name")); +} + +XMLElement* Document::addContactExclude(const std::string& body1Name, const std::string& body2Name) +{ + XMLElement* exclude = addNewElement(contact(), "exclude"); + + exclude->SetAttribute("body1", body1Name.c_str()); + exclude->SetAttribute("body2", body2Name.c_str()); + + return exclude; +} + + +XMLElement* Document::addNewElement(XMLElement* parent, const std::string& elemName, + const std::string& className, bool first) +{ + XMLElement* elem = NewElement(elemName.c_str()); + + if (!className.empty()) + { + elem->SetAttribute("class", className.c_str()); + } + else if (!newElementClass.empty() + && !(newElementClassExcludeBody && (isElement(parent, "body") || isElement(elem, "body"))) + && allowsClassAttr({parent->Value(), elemName})) + { + elem->SetAttribute("class", newElementClass.c_str()); + } + + if (first) + { + parent->InsertFirstChild(elem); + } + else + { + parent->InsertEndChild(elem); + } + return elem; +} + +XMLElement* Document::getOrCreateElement(XMLElement* parent, const std::string& elemName) +{ + XMLElement* elem = parent->FirstChildElement(elemName.c_str()); + if (!elem) + { + elem = addNewElement(parent, elemName); + } + return elem; +} + +XMLElement* Document::section(const std::string& name) +{ + return getOrCreateElement(root, name); +} + +void Document::setDummyMass(float value) +{ + dummyMass = value; +} + +void Document::setFloatCompPrecision(float value) +{ + floatCompPrecision = value; +} + + +XMLElement* Document::robotRootBody() const +{ + return robotRootBody_; +} + + +const std::set<Document::ElementType> Document::ELEM_NAMES_WITH_ATTR_CLASS = +{ + {"asset", "mesh" }, + {"asset", "material"}, + + {"body", "joint" }, + {"body", "geom" }, + {"body", "site" }, + {"body", "camera" }, + {"body", "light" }, + + {"contact", "pair" }, + + {"equality", "connect" }, + {"equality", "weld" }, + {"equality", "joint" }, + {"equality", "tendon" }, + {"equality", "distance" }, + + {"tendon", "spatial" }, + {"tendon", "fixed" }, + + {"actuator", "general" }, + {"actuator", "motor" }, + {"actuator", "position" }, + {"actuator", "velocity" }, + {"actuator", "cylinder" }, + {"actuator", "muscle" }, +}; + +bool Document::allowsClassAttr(const Document::ElementType& type) +{ + return ELEM_NAMES_WITH_ATTR_CLASS.find(type) != ELEM_NAMES_WITH_ATTR_CLASS.end(); +} + + diff --git a/VirtualRobot/XML/mjcf/MjcfDocument.h b/VirtualRobot/XML/mjcf/MjcfDocument.h new file mode 100644 index 0000000000000000000000000000000000000000..6f89d5c7c98c7737c2b6b64d27d0dc015612e86f --- /dev/null +++ b/VirtualRobot/XML/mjcf/MjcfDocument.h @@ -0,0 +1,204 @@ +#pragma once + +#include <memory> + +#include <Eigen/Eigen> + +#include <VirtualRobot/Robot.h> +#include <VirtualRobot/Util/xml/tinyxml2.h> + +#include "utils.h" + + +namespace VirtualRobot +{ +namespace mjcf +{ + + using XMLElement = tinyxml2::XMLElement; + + /** + * @brief A MJCF (Mujoco XML) document. + */ + class Document : public tinyxml2::XMLDocument + { + + public: + + /// Constructor. + Document(); + + + /// Set the precision for float comparison (used e.g. when comparing + /// to zero or identity). + void setFloatCompPrecision(float value); + /// Set the mass set in dummy inertial elements. + void setDummyMass(float value); + + + /// Set the name of the Mujoco model. + void setModelName(const std::string& name); + + /** + * @brief Set the class attribute of all new applicable elements + * (after calling this method). Pass an empty string to disable class attributes. + * @param excludeBody If true (default), the class will not be set on + * new body elements and its children (inertial, joint, ...). + * They should be the childclass attribute of the robot root body. + */ + void setNewElementClass(const std::string& className, bool excludeBody = true); + + + // Section elements (children of top-level 'mujoco' element). + XMLElement* compiler() { return section("compiler"); } + XMLElement* option() { return section("option"); } + XMLElement* size() { return section("size"); } + XMLElement* visual() { return section("visual"); } + XMLElement* statistic() { return section("statistic"); } + XMLElement* default_() { return section("default"); } + XMLElement* asset() { return section("asset"); } + XMLElement* worldbody() { return section("worldbody"); } + XMLElement* contact() { return section("contact"); } + XMLElement* equality() { return section("equality"); } + XMLElement* tendon() { return section("tendon"); } + XMLElement* actuator() { return section("actuator"); } + XMLElement* sensor() { return section("sensor"); } + XMLElement* keyframe() { return section("keyframe"); } + + + /// Adds a body element to the worldbody with the robot's name as name and childclass. + XMLElement* addRobotRootBodyElement(const std::string& robotName); + /// Get the (least recently added) robot root body. + XMLElement* robotRootBody() const; + + /// Add a new defaults class with a class name. + XMLElement* addDefaultsClass(const std::string& className); + + template <typename AttrT> + XMLElement* addDefaultAttr(XMLElement* defaultsClass, const std::string& elementName, + const std::string& attrName, const AttrT& attrValue); + + + /// Add a skybox texture asset (only one allowed). + XMLElement* addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2); + + /// Add a mocap body with the given name to the worldbody. + XMLElement* addMocapBody(const std::string& name, float geomSize); + + /// Add a body element to a parent from a RobotNode. + /// Adds inertial and joint element, if necessary. + XMLElement* addBodyElement(XMLElement* parent, RobotNodePtr node); + + /// Add an inertial element to a body from a RobotNode. + XMLElement* addInertialElement(XMLElement* body, RobotNodePtr node); + /// Add a dummy inertial element with small mass and identity inertia matrix. + XMLElement* addDummyInertial(XMLElement* body, bool first=false); + /// Add a joint element to a body from a RobotNode. + XMLElement* addJointElement(XMLElement* body, RobotNodePtr node); + /// Add a free joint element to a body. + XMLElement* addFreeJointElement(XMLElement* body); + + /// Add a geom to a body, referencing a mesh. + XMLElement* addGeomElement(XMLElement* body, const std::string& meshName); + /// Add a mesh asset with a name and a file. + XMLElement* addMeshElement(const std::string& name, const std::string& file, + const std::string& className = ""); + + /// Add a conact/exclude element between the given bodies. + XMLElement* addContactExclude(const XMLElement& body1, const XMLElement& body2); + /// Add a conact/exclude element between the given bodies. + XMLElement* addContactExclude(const std::string& body1Name, const std::string& body2Name); + + XMLElement* addActuatorMotorElement(const std::string& jointName); + XMLElement* addActuatorPositionElement(const std::string& jointName, float kp = 0); + XMLElement* addActuatorPositionElement( + const std::string& jointName, bool ctrlLimited, const Eigen::Vector2f& ctrlRange, float kp = 0); + XMLElement* addActuatorVelocityElement(const std::string& jointName, float kv = 0); + + XMLElement* addEqualityWeld(const std::string& name, const std::string& body1, const std::string& body2, + const std::string& className=""); + + /// Set the pos and quat attribute of an element. + void setElementPose(XMLElement* elem, const Eigen::Matrix4f& pose); + void setElementPos(XMLElement* elem, Eigen::Vector3f pos); + void setElementQuat(XMLElement* elem, const Eigen::Quaternionf& quat); + + /// Set the axis attribute of a joint. + void setJointAxis(XMLElement* joint, const Eigen::Vector3f& axis); + + + + private: + + /// (ParentTag, ElementTag) + using ElementType = std::pair<std::string, std::string>; + + /// Element types allowing a class attribute. + static const std::set<ElementType> ELEM_NAMES_WITH_ATTR_CLASS; + + /// Return true if the given ElementType allows for a class attribute. + static bool allowsClassAttr(const ElementType& type); + + + private: + + + + /// Add a new element with a name (tag) to a parent. + /// @param className If not empty, set the class attribute of the new element. + /// @param first If true, will be inserted as first, otherweise at end (default) + XMLElement* addNewElement(XMLElement* parent, const std::string& elemName, + const std::string& className = "", bool first = false); + + /// Return the first child element of parent with the given element name. + /// If it does not exist, create it. + XMLElement* getOrCreateElement(XMLElement* parent, const std::string& elemName); + + /// Gets the section element (child of root element) with a name. + /// If it does not exist, it is created. + XMLElement* section(const std::string& name); + + XMLElement* addActuatorShortcut( + const std::string& type, const std::string& name, const std::string& jointName, + const std::string& paramName = "", float paramValue = 0); + + + /// Precision when comparing floats (e.g. with zero). + float floatCompPrecision = 1e-6f; + /// Mass used for dummy inertial elements. + float dummyMass = 0.0001f; + + + /// The "mujoco" root element. + XMLElement* root; + + /// The wrapper element of the robot. + XMLElement* robotRootBody_; + + + /// The class added to new elements, if applicable. + std::string newElementClass = ""; + /// Indicate whether body elements and their children shall be + /// exluded from setting the class attribute. + bool newElementClassExcludeBody = true; + + + }; + + template<typename AttrT> + XMLElement* Document::addDefaultAttr( + XMLElement* defaultsClass, const std::string& elementName, + const std::string& attrName, const AttrT& attrValue) + { + assertElementIs(defaultsClass, "default"); + + XMLElement* element = getOrCreateElement(defaultsClass, elementName); + element->SetAttribute(attrName.c_str(), attrValue); + + return element; + } + + using DocumentPtr = std::unique_ptr<Document>; + +} +} diff --git a/VirtualRobot/XML/mjcf/MujocoIO.cpp b/VirtualRobot/XML/mjcf/MujocoIO.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8e0bb93142b91f5c36843b55faa3d84db98259e0 --- /dev/null +++ b/VirtualRobot/XML/mjcf/MujocoIO.cpp @@ -0,0 +1,511 @@ +#include "MujocoIO.h" +#include "utils.h" + +#include <VirtualRobot/RobotNodeSet.h> +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/VirtualRobotException.h> +#include <VirtualRobot/Visualization/TriMeshModel.h> + + +using namespace VirtualRobot; +using namespace mjcf; +namespace tx = tinyxml2; +namespace fs = boost::filesystem; + + + +MujocoIO::MujocoIO(RobotPtr robot) : robot(robot) +{ + THROW_VR_EXCEPTION_IF(!robot, "Given RobotPtr robot is null."); +} + +void MujocoIO::saveMJCF(const std::string& filename, const std::string& basePath, + const std::string& meshRelDir) +{ + THROW_VR_EXCEPTION_IF(filename.empty(), "Given filename is empty."); + + setPaths(filename, basePath, meshRelDir); + + document.reset(new mjcf::Document()); + document->setModelName(robot->getName()); + + makeCompiler(); + + makeDefaultsGroup(); + document->setNewElementClass(robot->getName(), true); + + addSkybox(); + + if (withMocapBody) + { + std::cout << "Adding mocap body ..." << std::endl; + addMocapBody(); + } + + std::cout << "Creating bodies structure ..." << std::endl; + makeNodeBodies(); + + std::cout << "Adding meshes and geoms ..." << std::endl; + addNodeBodyMeshes(); + + bool print = false; + if (print) + { + std::cout << "===========================" << std::endl + << "Current model: " << std::endl + << "--------------" << std::endl; + document->Print(); + std::cout << "===========================" << std::endl; + } + + std::cout << "Merging massless bodies ..." << std::endl; + masslessBodySanitizer.sanitize(); + + std::cout << "Adding contact excludes ..." << std::endl; + addContactExcludes(); + + std::cout << "Adding actuators ..." << std::endl; + addActuators(); + + std::cout << "Scaling lengths by " << lengthScaling << " ..." << std::endl; + scaleLengths(document->robotRootBody()); + + std::cout << "Done." << std::endl; + + if (print) + { + std::cout << std::endl; + std::cout << "===========================" << std::endl + << "Output file: " << std::endl + << "------------" << std::endl; + document->Print(); + std::cout << "===========================" << std::endl; + } + + assert(!outputFileName.empty()); + std::cout << "Writing to " << (outputDirectory / outputFileName) << std::endl; + document->SaveFile((outputDirectory / outputFileName).c_str()); +} + +void MujocoIO::setPaths(const std::string& filename, const std::string& basePath, const std::string& meshRelDir) +{ + outputDirectory = basePath; + outputFileName = filename; + outputMeshRelDirectory = meshRelDir; + + ensureDirectoriesExist(); +} + +void MujocoIO::ensureDirectoriesExist() +{ + auto ensureDirExists = [](const fs::path& dir, const std::string& errMsgName) + { + if (!fs::is_directory(dir)) + { + std::cout << "Creating directory: " << dir << std::endl; + bool success = fs::create_directories(dir); + THROW_VR_EXCEPTION_IF(!success, "Could not create " << errMsgName << ": " << dir); + } + }; + + ensureDirExists(outputDirectory, "output directory"); + ensureDirExists(outputMeshDirectory(), "output mesh directory"); +} + +void MujocoIO::makeCompiler() +{ + document->compiler()->SetAttribute("angle", "radian"); + document->compiler()->SetAttribute("balanceinertia", "true"); +} + +void MujocoIO::makeDefaultsGroup() +{ + XMLElement* defaultsClass = document->addDefaultsClass(robot->getName()); + + std::stringstream comment; + comment << "Add default values for " << robot->getName() << " here."; + defaultsClass->InsertFirstChild(document->NewComment(comment.str().c_str())); + + document->addDefaultAttr(defaultsClass, "joint", "frictionloss", 1); + document->addDefaultAttr(defaultsClass, "joint", "damping", 0); + document->addDefaultAttr(defaultsClass, "geom", "condim", 4); + document->addDefaultAttr(defaultsClass, "position", "kp", 1); + document->addDefaultAttr(defaultsClass, "velocity", "kv", 1); +} + + +void MujocoIO::addSkybox() +{ + document->addSkyboxTexture(Eigen::Vector3f(.8f, .9f, .95f), + Eigen::Vector3f(.4f, .6f, .8f)); +} + + +void MujocoIO::addMocapBody() +{ + std::string className = "mocap"; + float geomSize = 0.01f; + + std::string bodyName; + { + std::stringstream ss; + ss << robot->getName() << "_Mocap"; + bodyName = ss.str(); + } + + // add defaults class + XMLElement* defClass = document->addDefaultsClass(className); + + document->addDefaultAttr(defClass, "geom", "rgba", + toAttr(Eigen::Vector4f(.9f, .5f, .5f, .5f)).c_str()); + + document->addDefaultAttr(defClass, "equality", "solimp", + toAttr(Eigen::Vector3f{.95f, .99f, .001f}).c_str()); + document->addDefaultAttr(defClass, "equality", "solref", + toAttr(Eigen::Vector2f{ .02f, 1.f}).c_str()); + + // add body + XMLElement* mocap = document->addMocapBody(bodyName, geomSize); + mocap->SetAttribute("childclass", className.c_str()); + + // add equality weld constraint + document->addEqualityWeld(bodyName, robot->getName(), bodyName, className); +} + + +void MujocoIO::makeNodeBodies() +{ + nodeBodies.clear(); + + RobotNodePtr rootNode = robot->getRootNode(); + assert(rootNode); + + // add root + XMLElement* robotRootBody = document->addRobotRootBodyElement(robot->getName()); + + if (withMocapBody) + { + document->addDummyInertial(robotRootBody); + XMLElement* joint = document->addFreeJointElement(robotRootBody); + joint->SetAttribute("name", robot->getName().c_str()); + } + + XMLElement* root = document->addBodyElement(robotRootBody, rootNode); + nodeBodies[rootNode->getName()] = root; + assert(root); + + for (RobotNodePtr node : robot->getRobotNodes()) + { + addNodeBody(node); + } +} + +void MujocoIO::addNodeBodyMeshes() +{ + bool meshlabserverAviable = system("which meshlabserver > /dev/null 2>&1") == 0; + bool notAvailableReported = false; + + for (RobotNodePtr node : robot->getRobotNodes()) + { + VisualizationNodePtr visualization = node->getVisualization(SceneObject::VisualizationType::Full); + + if (!visualization) + { + continue; + } + + std::cout << t << "Node " << node->getName() << ":\t"; + + fs::path srcMeshPath = visualization->getFilename(); + + fs::path dstMeshFileName = srcMeshPath.filename(); + dstMeshFileName.replace_extension("stl"); + fs::path dstMeshPath = outputMeshDirectory() / dstMeshFileName; + + if (!fs::exists(dstMeshPath)) + { + if (srcMeshPath.extension().string() != "stl") + { + std::cout << "Converting to .stl: " << srcMeshPath; + + if (!meshlabserverAviable) + { + if (!notAvailableReported) + { + std::cout << std::endl + << "Command 'meshlabserver' not available, " + "cannot convert meshes." << std::endl; + notAvailableReported = true; + } + continue; + } + + // meshlabserver available + std::stringstream convertCommand; + convertCommand << "meshlabserver" + << " -i " << srcMeshPath + << " -o " << dstMeshPath; + + // run command + std::cout << "----------------------------------------------------------" << std::endl; + int r = system(convertCommand.str().c_str()); + std::cout << "----------------------------------------------------------" << std::endl; + if (r != 0) + { + std::cout << "Command returned with error: " << r << "\n" + << "Command was: " << convertCommand.str() << std::endl; + } + } + else + { + std::cout << "Copying: " << srcMeshPath << "\n" + << " to: " << dstMeshPath; + fs::copy_file(srcMeshPath, dstMeshPath); + } + } + else + { + std::cout << "skipping (" << outputMeshRelDirectory / dstMeshFileName + << " already exists)"; + } + std::cout << std::endl; + + + + // add asset + std::string meshName = node->getName(); + document->addMeshElement(meshName, fs::absolute(dstMeshPath).string()); + + // add geom to body + XMLElement* body = nodeBodies[node->getName()]; + document->addGeomElement(body, meshName); + } +} + + + +XMLElement* MujocoIO::addNodeBody(RobotNodePtr node) +{ + XMLElement* element = nodeBodies[node->getName()]; + if (element) + { + // break recursion + return element; + } + + XMLElement* parent = nodeBodies[node->getParent()->getName()]; + if (!parent) + { + parent = addNodeBody(robot->getRobotNode(node->getParent()->getName())); + } + + element = document->addBodyElement(parent, node); + nodeBodies[node->getName()] = element; + + return element; +} + +struct ParentChildContactExcludeVisitor : public tinyxml2::XMLVisitor +{ + + ParentChildContactExcludeVisitor(Document& document) : document(document) {} + ~ParentChildContactExcludeVisitor() override = default; + + bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override; + + Document& document; ///< The document. + bool firstSkipped = false; ///< Used to skip the root element. +}; + +bool ParentChildContactExcludeVisitor::VisitEnter(const tinyxml2::XMLElement& body, const tinyxml2::XMLAttribute*) +{ + if (!isElement(body, "body")) + { + return true; + } + + if (!firstSkipped) + { + firstSkipped = true; + return true; + } + + const XMLElement* parent = body.Parent()->ToElement(); + assert(parent); + + document.addContactExclude(*parent, body); + + return true; +} + + +void MujocoIO::addContactExcludes() +{ + RobotNodePtr rootNode = robot->getRootNode(); + + std::vector<std::pair<std::string, std::string>> excludePairs; + + for (RobotNodePtr node : robot->getRobotNodes()) + { + for (std::string& ignore : node->getPhysics().ignoreCollisions) + { + // I found an <IgnoreCollision> element referring to a non-existing node. + // => check node existence here + if (robot->hasRobotNode(ignore)) + { + excludePairs.push_back({node->getName(), ignore}); + } + } + } + + // resolve body names and add exludes + for (auto& excludePair : excludePairs) + { + std::string body1 = masslessBodySanitizer.getMergedBodyName(excludePair.first); + std::string body2 = masslessBodySanitizer.getMergedBodyName(excludePair.second); + document->addContactExclude(body1, body2); + } + + ParentChildContactExcludeVisitor visitor(*document); + document->robotRootBody()->Accept(&visitor); +} + +void MujocoIO::addActuators() +{ + std::vector<const mjcf::XMLElement*> jointElements = getAllElements("joint"); + + for (auto joint : jointElements) + { + std::string name = joint->Attribute("name"); + switch (actuatorType) + { + case ActuatorType::MOTOR: + document->addActuatorMotorElement(name); + break; + + case ActuatorType::POSITION: + { + XMLElement* act = document->addActuatorPositionElement(name); + + const char* limited = joint->Attribute("limited"); + if (limited) + { + act->SetAttribute("ctrllimited", limited); + + const char* range = joint->Attribute("range"); + if (range) + { + act->SetAttribute("ctrlrange", range); + } + } + } + break; + + case ActuatorType::VELOCITY: + document->addActuatorVelocityElement(name); + break; + } + } +} + +void MujocoIO::scaleLengths(XMLElement* elem) +{ + assert(elem); + + if (isElement(elem, "joint")) + { + if (isAttr(elem, "type", "slide") && hasAttr(elem, "range")) + { + std::cout << t << "Scaling range of slide joint " + << elem->Attribute("name") << std::endl; + + Eigen::Vector2f range = strToVec2(elem->Attribute("range")); + range *= lengthScaling; + setAttr(elem, "range", range); + } + } + else if (isElement(elem, "position") + //&& isElement(elem.Parent()->ToElement(), "actuator") + && hasAttr(elem, "ctrlrange")) + { + std::cout << t << "Scaling ctrlrange of position actuator " + << elem->Attribute("name") << std::endl; + + Eigen::Vector2f ctrlrange = strToVec2(elem->Attribute("ctrlrange")); + ctrlrange *= lengthScaling; + setAttr(elem, "ctrlrange", ctrlrange); + } + else if (hasAttr(elem, "pos")) + { + std::cout << t << "Scaling pos of " << elem->Value() << " "; + if (hasAttr(elem, "name")) + { + std::cout << "'" << elem->Attribute("name") << "'"; + } + else + { + std::cout << "element"; + } + std::cout << std::endl; + + Eigen::Vector3f pos = strToVec(elem->Attribute("pos")); + pos *= lengthScaling; + setAttr(elem, "pos", pos); + } + + + for (XMLElement* child = elem->FirstChildElement(); + child; + child = child->NextSiblingElement()) + { + scaleLengths(child); + } +} + + +struct ListElementsVisitor : public tinyxml2::XMLVisitor +{ + + ListElementsVisitor(const std::string& elementName) : elementName(elementName) {} + ~ListElementsVisitor() override = default; + + // XMLVisitor interface + bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override; + + const std::vector<const tinyxml2::XMLElement*>& getFoundElements() const; + + std::string elementName; + std::vector<const tinyxml2::XMLElement*> foundElements; +}; + +bool ListElementsVisitor::VisitEnter(const tinyxml2::XMLElement& elem, const tinyxml2::XMLAttribute*) +{ + if (isElement(elem, elementName)) + { + foundElements.push_back(&elem); + } + return true; +} + +std::vector<const mjcf::XMLElement*> MujocoIO::getAllElements(const std::string& elemName) +{ + ListElementsVisitor visitor(elemName); + document->worldbody()->Accept(&visitor); + return visitor.foundElements; +} + +void MujocoIO::setLengthScaling(float value) +{ + lengthScaling = value; +} + +void MujocoIO::setActuatorType(ActuatorType value) +{ + actuatorType = value; +} + +void MujocoIO::setWithMocapBody(bool enabled) +{ + withMocapBody = enabled; +} + diff --git a/VirtualRobot/XML/mjcf/MujocoIO.h b/VirtualRobot/XML/mjcf/MujocoIO.h new file mode 100644 index 0000000000000000000000000000000000000000..7ab57bb0f49ab5ea2264b4d8e2b3f7efba786fb5 --- /dev/null +++ b/VirtualRobot/XML/mjcf/MujocoIO.h @@ -0,0 +1,136 @@ +#pragma once + +#include <boost/filesystem.hpp> + +#include <VirtualRobot/Robot.h> + +#include "MjcfDocument.h" +#include "MasslessBodySanitizer.h" +#include "utils.h" + + +namespace VirtualRobot +{ +namespace mjcf +{ + + class MujocoIO + { + public: + + /// Constructor. + /// @throws VirtualRobotException if robot is null + MujocoIO(RobotPtr robot); + + /** + * @brief Create a Mujoco XML (MJCF) document for the given robot. + * @param filename the output filename (without directory) + * @param basePath the output directory + * @param meshRelDir the directory relative to basePath where meshes shall be placed + */ + void saveMJCF(const std::string& filename, const std::string& basePath, + const std::string& meshRelDir); + + + /// Set the scaling for lenghts. + void setLengthScaling(float value); + + /// Set the actuator type. + void setActuatorType(ActuatorType value); + + /** + * @brief Enable or disable adding of a mocap body controlling the robot pose. + * + * @param enabled If true: + * - Add a free joint to the robot root body mocap body + * - Add a mocap body in the world body (called <RobotName>_Mocap) + * - Add a weld constraint welding them together. + */ + void setWithMocapBody(bool enabled); + + + private: + + /// Set the output path members. + void setPaths(const std::string& filename, const std::string& basePath, + const std::string& meshRelDir); + /// Create output directories if the do not exist. + void ensureDirectoriesExist(); + + /// Make the compiler section. + void makeCompiler(); + /// Add a defaults group for the robot. + void makeDefaultsGroup(); + /// Add a skybox texture asset. + void addSkybox(); + + /// Add a mocap body with defaults group. + void addMocapBody(); + + /// Construct the body structure corresponding to the robot nodes. + void makeNodeBodies(); + /// Add a body element for a robot node. If its parent does not exist, + /// create the parent body first. + mjcf::XMLElement* addNodeBody(RobotNodePtr node); + + /// Convert meshes and add mesh assets for robot nodes with visualization. + void addNodeBodyMeshes(); + + /// Add contact exclude elements for IgnoreCollision elements. + void addContactExcludes(); + + /// Add actuators for all joints. + void addActuators(); + + /// Scale all lengths by lengthScaling. + void scaleLengths(XMLElement* elem); + + std::vector<const mjcf::XMLElement*> getAllElements(const std::string& elemName); + + + // Paremeters + + /// Scaling for lengths, such as positions and translations. + float lengthScaling = 0.001f; + /// The actuator type. + ActuatorType actuatorType = ActuatorType::MOTOR; + + /// Add a mocap + bool withMocapBody = false; + + + // Paths + + boost::filesystem::path outputDirectory; + boost::filesystem::path outputFileName; + boost::filesystem::path outputMeshRelDirectory; + boost::filesystem::path outputMeshDirectory() { return outputDirectory / outputMeshRelDirectory; } + + + // Input + + /// The input robot. + RobotPtr robot; + + + // Output + + /// The built MJCF document. + DocumentPtr document = nullptr; + + + // Processing + + /// Sanitizes massless bodies. + mjcf::MasslessBodySanitizer masslessBodySanitizer {document, robot}; + + /// Map of robot node names to XML elements. + std::map<std::string, mjcf::XMLElement*> nodeBodies; + + + const std::string t = "| "; + + }; + +} +} diff --git a/VirtualRobot/XML/mjcf/utils.cpp b/VirtualRobot/XML/mjcf/utils.cpp new file mode 100644 index 0000000000000000000000000000000000000000..be83738e4487d531078f6a3f2fc881b4e7ed99f2 --- /dev/null +++ b/VirtualRobot/XML/mjcf/utils.cpp @@ -0,0 +1,139 @@ +#include "utils.h" + +#include <algorithm> +#include <map> + + +namespace VirtualRobot +{ +namespace mjcf +{ + +ActuatorType toActuatorType(std::string str) +{ + static const std::map<std::string, ActuatorType> map { + {"motor", ActuatorType::MOTOR}, + {"position", ActuatorType::POSITION}, + {"velocity", ActuatorType::VELOCITY} + }; + std::transform(str.begin(), str.end(), str.begin(), ::tolower); + return map.at(str); +} + + + +bool isElement(const XMLElement* elem, const char* tag) +{ + return isElement(*elem, tag); +} + +bool isElement(const XMLElement* elem, const std::string& tag) +{ + return isElement(*elem, tag); +} + +bool isElement(const XMLElement& elem, const char* tag) +{ + return std::strcmp(elem.Value(), tag) == 0; +} + +bool isElement(const XMLElement& elem, const std::string& tag) +{ + return std::strcmp(elem.Value(), tag.c_str()) == 0; +} + + +bool hasElementChild(const XMLElement* elem, const std::string& elemName) +{ + return elem->FirstChildElement(elemName.c_str()) != nullptr; +} + +bool hasMass(const XMLElement* body) +{ + assertElementIsBody(body); + return hasElementChild(body, "geom") || hasElementChild(body, "inertial"); +} + + + +std::string toAttr(bool b) +{ + static const std::string strings[] = { "false", "true" }; + return strings[int(b)]; +} + +std::string toAttr(const Eigen::Quaternionf& quat) +{ + std::stringstream ss; + ss << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z(); + return ss.str(); +} + + +Eigen::Vector2f strToVec2(const char* string) +{ + Eigen::Vector2f v; + sscanf(string, "%f %f", &v(0), &v(1)); + return v; +} + +Eigen::Vector3f strToVec(const char* string) +{ + Eigen::Vector3f v; + sscanf(string, "%f %f %f", &v(0), &v(1), &v(2)); + return v; +} + +Eigen::Quaternionf strToQuat(const char* string) +{ + Eigen::Quaternionf q; + sscanf(string, "%f %f %f %f", &q.w(), &q.x(), &q.y(), &q.z()); + return q; +} + + +std::size_t commonPrefixLength(const std::string& a, const std::string& b) +{ + const std::string* smaller = &a; + const std::string* bigger = &b; + if (b.size() < a.size()) + { + std::swap(smaller, bigger); + } + + auto mismatch = std::mismatch(smaller->begin(), smaller->end(), + bigger->begin()).first; + return std::size_t(std::distance(smaller->begin(), mismatch)); +} + + +void assertElementIsBody(const XMLElement* elem) +{ + assertElementIs(elem, "body"); +} + +bool hasAttr(const XMLElement* elem, const std::string& attrName) +{ + return hasAttr(*elem, attrName); +} + +bool hasAttr(const XMLElement& elem, const std::string& attrName) +{ + return elem.Attribute(attrName.c_str()); +} + +bool isAttr(const XMLElement* elem, const std::string& attrName, const std::string& attrValue) +{ + return isAttr(*elem, attrName, attrValue); +} + +bool isAttr(const XMLElement& elem, const std::string& attrName, const std::string& attrValue) +{ + return hasAttr(elem, attrName) && elem.Attribute(attrName.c_str()) == attrValue; +} + + + + +} +} diff --git a/VirtualRobot/XML/mjcf/utils.h b/VirtualRobot/XML/mjcf/utils.h new file mode 100644 index 0000000000000000000000000000000000000000..0adcb4b5400d5ff61c51e4a70d275bcfc09ab758 --- /dev/null +++ b/VirtualRobot/XML/mjcf/utils.h @@ -0,0 +1,103 @@ +#pragma once + +#include <VirtualRobot/Util/xml/tinyxml2.h> +#include <string> +#include <Eigen/Eigen> + + +namespace VirtualRobot +{ +namespace mjcf +{ + +enum class ActuatorType +{ + MOTOR, POSITION, VELOCITY +}; + +ActuatorType toActuatorType(std::string str); + + +using XMLElement = tinyxml2::XMLElement; + + +bool isElement(const XMLElement* elem, const char* tag); +bool isElement(const XMLElement& elem, const char* tag); +bool isElement(const XMLElement* elem, const std::string& tag); +bool isElement(const XMLElement& elem, const std::string& tag); + +bool hasElementChild(const XMLElement* elem, const std::string& elemName); + +bool hasMass(const XMLElement* body); + + +bool hasAttr(const XMLElement* elem, const std::string& attrName); +bool hasAttr(const XMLElement& elem, const std::string& attrName); +bool isAttr(const XMLElement* elem, const std::string& attrName, const std::string& attrValue); +bool isAttr(const XMLElement& elem, const std::string& attrName, const std::string& attrValue); + +template <typename AttrT> +void setAttr(XMLElement* elem, const std::string& name, const AttrT& value); +template <typename AttrT> +void setAttr(XMLElement& elem, const std::string& name, const AttrT& value); + +// Convert to MJCF XML attribute format. +std::string toAttr(bool b); +template <int dim> +std::string toAttr(const Eigen::Matrix<float, dim, 1>& v); +std::string toAttr(const Eigen::Quaternionf& v); + +// Convert from MJCF XML attribute. +Eigen::Vector2f strToVec2(const char* string); +Eigen::Vector3f strToVec(const char* string); +Eigen::Quaternionf strToQuat(const char* string); + +// Get lenght of common prefix of two strings (was used for mergin body names). +std::size_t commonPrefixLength(const std::string& a, const std::string& b); + + +// Assert that isElement(elem, tag). +template <class StringT> +void assertElementIs(const XMLElement* elem, const StringT tag); + + +// Assert that isElement(elem, "body"). +void assertElementIsBody(const XMLElement* elem); + + + + +// DEFINITIONS of templated methods + + +template <int dim> +std::string toAttr(const Eigen::Matrix<float, dim, 1>& v) +{ + static const Eigen::IOFormat iofVector {7, 0, "", " ", "", "", "", ""}; + + std::stringstream ss; + ss << v.format(iofVector); + return ss.str(); +} + +template <typename AttrT> +void setAttr(XMLElement* elem, const std::string& name, const AttrT& value) +{ + setAttr(*elem, name, value); +} + +template <typename AttrT> +void setAttr(XMLElement& elem, const std::string& name, const AttrT& value) +{ + elem.SetAttribute(name.c_str(), toAttr(value).c_str()); +} + +template <class StringT> +void assertElementIs(const XMLElement* elem, const StringT tag) +{ + assert(isElement(elem, tag)); +} + +} +} + diff --git a/VirtualRobot/examples/CMakeLists.txt b/VirtualRobot/examples/CMakeLists.txt index 3d80def1f4fb3b1403cb6af047d98caef4b91e19..d365320618ec6ce8da9badb376a3a8f37ac4c909 100644 --- a/VirtualRobot/examples/CMakeLists.txt +++ b/VirtualRobot/examples/CMakeLists.txt @@ -13,15 +13,16 @@ endif () ADD_SUBDIRECTORY(loadRobot) ADD_SUBDIRECTORY(CameraViewer) +ADD_SUBDIRECTORY(GenericIK) +ADD_SUBDIRECTORY(GraspEditor) +ADD_SUBDIRECTORY(Jacobi) +ADD_SUBDIRECTORY(loadURDFRobot) +ADD_SUBDIRECTORY(reachability) +ADD_SUBDIRECTORY(ReachabilityMap) ADD_SUBDIRECTORY(RobotViewer) ADD_SUBDIRECTORY(RobotViewerOSG) ADD_SUBDIRECTORY(SceneViewer) -ADD_SUBDIRECTORY(Jacobi) -ADD_SUBDIRECTORY(GenericIK) -ADD_SUBDIRECTORY(reachability) +ADD_SUBDIRECTORY(Simox2Mjcf) ADD_SUBDIRECTORY(stability) -ADD_SUBDIRECTORY(GraspEditor) -ADD_SUBDIRECTORY(ReachabilityMap) -ADD_SUBDIRECTORY(loadURDFRobot) diff --git a/VirtualRobot/examples/CameraViewer/showCamWindow.cpp b/VirtualRobot/examples/CameraViewer/showCamWindow.cpp index 4e3cc38beccdedf675f7abf8cb855cfc7e3a172c..beba7a64243afca4140fdb8516938e3d22329555 100644 --- a/VirtualRobot/examples/CameraViewer/showCamWindow.cpp +++ b/VirtualRobot/examples/CameraViewer/showCamWindow.cpp @@ -8,7 +8,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -31,14 +31,14 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; showCamWindow::showCamWindow(std::string& sRobotFilename, std::string& cam1Name, std::string& cam2Name) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; //this->setCaption(QString("ShowRobot - KIT - Humanoids Group")); //resize(1100, 768); - cam2Renderer = NULL; - cam2Buffer = NULL; - cam2DepthBuffer = NULL; + cam2Renderer = nullptr; + cam2Buffer = nullptr; + cam2DepthBuffer = nullptr; useColModel = false; VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(sRobotFilename); @@ -111,9 +111,9 @@ showCamWindow::~showCamWindow() sceneSep->unref(); cam1VoxelSep->unref(); - UI.cam1->setScene(NULL); + UI.cam1->setScene(nullptr); visuObjects.emplace_back(VirtualRobot::Obstacle::createSphere(400.0f)); - UI.cam2->setScene(NULL); + UI.cam2->setScene(nullptr); delete [] cam2Buffer; delete [] cam2DepthBuffer; @@ -212,7 +212,7 @@ void showCamWindow::rebuildVisualization() SceneObject::VisualizationType colModel = useColModel ? SceneObject::Collision : SceneObject::Full; visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -256,9 +256,9 @@ void showCamWindow::updateJointBox() { UI.comboBoxJoint->clear(); - for (unsigned int i = 0; i < currentRobotNodes.size(); i++) + for (auto & currentRobotNode : currentRobotNodes) { - UI.comboBoxJoint->addItem(QString(currentRobotNodes[i]->getName().c_str())); + UI.comboBoxJoint->addItem(QString(currentRobotNode->getName().c_str())); } } @@ -275,9 +275,9 @@ void showCamWindow::updateRNSBox() UI.comboBoxRobotNodeSet->clear(); UI.comboBoxRobotNodeSet->addItem(QString("<All>")); - for (unsigned int i = 0; i < robotNodeSets.size(); i++) + for (auto & robotNodeSet : robotNodeSets) { - UI.comboBoxRobotNodeSet->addItem(QString(robotNodeSets[i]->getName().c_str())); + UI.comboBoxRobotNodeSet->addItem(QString(robotNodeSet->getName().c_str())); } } @@ -409,7 +409,7 @@ void showCamWindow::loadRobot() { QFileInfo fileInfo(m_sRobotFilename.c_str()); std::string suffix(fileInfo.suffix().toLatin1()); - RobotImporterFactoryPtr importer = RobotImporterFactory::fromFileExtension(suffix, NULL); + RobotImporterFactoryPtr importer = RobotImporterFactory::fromFileExtension(suffix, nullptr); if (!importer) { @@ -442,11 +442,11 @@ void showCamWindow::updateCameras() { cam1.reset(); cam2.reset(); - cam2Renderer = NULL; + cam2Renderer = nullptr; delete []cam2Buffer; - cam2Buffer = NULL; + cam2Buffer = nullptr; delete [] cam2DepthBuffer; - cam2DepthBuffer = NULL; + cam2DepthBuffer = nullptr; if (!robot) { diff --git a/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp b/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp index 3237ac7f26394a4e2acaa9fb3e2242167555da8e..012796c139723f37a10441d2d483ffabe39ffff4 100644 --- a/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp +++ b/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp @@ -38,7 +38,7 @@ #include <Inventor/nodes/SoUnits.h> #include <Inventor/nodes/SoSphere.h> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <sstream> @@ -49,7 +49,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; ConstrainedIKWindow::ConstrainedIKWindow(std::string& sRobotFilename) - : QMainWindow(NULL) + : QMainWindow(nullptr) { robotFilename = sRobotFilename; sceneSep = new SoSeparator(); @@ -165,7 +165,7 @@ void ConstrainedIKWindow::collisionModel() robotSep->removeAllChildren(); boost::shared_ptr<CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(SceneObject::Full); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -216,12 +216,12 @@ void ConstrainedIKWindow::updateKCBox() robot->getRobotNodeSets(rns); kinChains.clear(); - for (unsigned int i = 0; i < rns.size(); i++) + for (auto & rn : rns) { - if (rns[i]->isKinematicChain()) + if (rn->isKinematicChain()) { - UI.comboBoxKC->addItem(QString(rns[i]->getName().c_str())); - kinChains.push_back(rns[i]); + UI.comboBoxKC->addItem(QString(rn->getName().c_str())); + kinChains.push_back(rn); } } } diff --git a/VirtualRobot/examples/GenericIK/GenericIKWindow.cpp b/VirtualRobot/examples/GenericIK/GenericIKWindow.cpp index c5cc008d624e97570b70399befdbfedcbbcf891a..38136fab9939e9c33c8a5e724c6f3826990dcded 100644 --- a/VirtualRobot/examples/GenericIK/GenericIKWindow.cpp +++ b/VirtualRobot/examples/GenericIK/GenericIKWindow.cpp @@ -8,7 +8,7 @@ #include "VirtualRobot/IK/constraints/PoseConstraint.h" #endif -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <sstream> @@ -19,7 +19,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; GenericIKWindow::GenericIKWindow(std::string& sRobotFilename) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; //this->setCaption(QString("ShowRobot - KIT - Humanoids Group")); @@ -195,7 +195,7 @@ void GenericIKWindow::collisionModel() SceneObject::VisualizationType colModel = useColModel ? SceneObject::Collision : SceneObject::Full; boost::shared_ptr<CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -246,12 +246,12 @@ void GenericIKWindow::updateKCBox() robot->getRobotNodeSets(rns); kinChains.clear(); - for (unsigned int i = 0; i < rns.size(); i++) + for (auto & rn : rns) { - if (rns[i]->isKinematicChain()) + if (rn->isKinematicChain()) { - UI.comboBoxKC->addItem(QString(rns[i]->getName().c_str())); - kinChains.push_back(rns[i]); + UI.comboBoxKC->addItem(QString(rn->getName().c_str())); + kinChains.push_back(rn); } } } @@ -428,9 +428,9 @@ void GenericIKWindow::solve() cout << "Joint values:" << endl; std::vector<RobotNodePtr> nodes = kc->getAllRobotNodes(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - cout << nodes[i]->getJointValue() << endl; + cout << node->getJointValue() << endl; } /* diff --git a/VirtualRobot/examples/Jacobi/JacobiWindow.cpp b/VirtualRobot/examples/Jacobi/JacobiWindow.cpp index 6fa2a35f5367cea99f29e0be98fbf1d1bf0ecf87..ed322b0c1b06fc43de8e91c6ba497548c757be3b 100644 --- a/VirtualRobot/examples/Jacobi/JacobiWindow.cpp +++ b/VirtualRobot/examples/Jacobi/JacobiWindow.cpp @@ -4,7 +4,7 @@ #include "VirtualRobot/EndEffector/EndEffector.h" #include "VirtualRobot/IK/DifferentialIK.h" -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <sstream> @@ -15,7 +15,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; JacobiWindow::JacobiWindow(std::string& sRobotFilename) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; //this->setCaption(QString("ShowRobot - KIT - Humanoids Group")); @@ -279,7 +279,7 @@ void JacobiWindow::collisionModel() SceneObject::VisualizationType colModel = useColModel ? SceneObject::Collision : SceneObject::Full; boost::shared_ptr<CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -330,12 +330,12 @@ void JacobiWindow::updateKCBox() robot->getRobotNodeSets(rns); kinChains.clear(); - for (unsigned int i = 0; i < rns.size(); i++) + for (auto & rn : rns) { - if (rns[i]->isKinematicChain()) + if (rn->isKinematicChain()) { - UI.comboBoxKC->addItem(QString(rns[i]->getName().c_str())); - kinChains.push_back(rns[i]); + UI.comboBoxKC->addItem(QString(rn->getName().c_str())); + kinChains.push_back(rn); } } } @@ -369,26 +369,26 @@ void JacobiWindow::selectKC(int nr) std::vector<RobotNodePtr> nodes = kc->getAllRobotNodes(); elbow.reset(); - for (size_t i = 0; i < nodes.size(); i++) + for (auto & node : nodes) { - if ((nodes[i])->getName() == std::string("Elbow L")) + if (node->getName() == std::string("Elbow L")) { - elbow = nodes[i]; + elbow = node; } - if ((nodes[i])->getName() == std::string("Elbow R")) + if (node->getName() == std::string("Elbow R")) { - elbow = nodes[i]; + elbow = node; } - if ((nodes[i])->getName() == std::string("TCP L")) + if (node->getName() == std::string("TCP L")) { - elbow = nodes[i]; + elbow = node; } - if ((nodes[i])->getName() == std::string("TCP R")) + if (node->getName() == std::string("TCP R")) { - elbow = nodes[i]; + elbow = node; } } diff --git a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp index 4c2d65fd0d1eae321222a6d165fab203a752c3f8..f050de8cfffefbcc15359e24ed3da7840ec1cee4 100644 --- a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp +++ b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp @@ -10,7 +10,7 @@ #include <VirtualRobot/Workspace/WorkspaceGrid.h> #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -29,7 +29,7 @@ float TIMER_MS = 30.0f; //#define ENDLESS ReachabilityMapWindow::ReachabilityMapWindow(std::string& sRobotFile, std::string& reachFile, std::string& objFile, std::string& eef) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -338,7 +338,7 @@ void ReachabilityMapWindow::buildRobotVisu() } boost::shared_ptr<VirtualRobot::CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -361,7 +361,7 @@ void ReachabilityMapWindow::buildObjectVisu() } boost::shared_ptr<VirtualRobot::CoinVisualization> visualization = graspObject->getVisualization<CoinVisualization>(); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -485,9 +485,9 @@ void ReachabilityMapWindow::updateEEFBox() std::vector<EndEffectorPtr> eefs = robot->getEndEffectors(); - for (unsigned int i = 0; i < eefs.size(); i++) + for (auto & eef : eefs) { - UI.comboBoxEEF->addItem(QString(eefs[i]->getName().c_str())); + UI.comboBoxEEF->addItem(QString(eef->getName().c_str())); } selectEEF(0); diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp index e17e88ed7a3f8543e1a22c196c6f789f371c4533..bc9e57648a70371ae83b89eaddecf66aa3ea57b1 100644 --- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp +++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp @@ -9,7 +9,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -26,7 +26,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; showRobotWindow::showRobotWindow(std::string& sRobotFilename) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; //this->setCaption(QString("ShowRobot - KIT - Humanoids Group")); @@ -255,7 +255,7 @@ void showRobotWindow::rebuildVisualization() SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full; visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -287,10 +287,10 @@ void showRobotWindow::showSensors() std::vector<SensorPtr> sensors = robot->getSensors(); - for (size_t i = 0; i < sensors.size(); i++) + for (auto & sensor : sensors) { - sensors[i]->setupVisualization(showSensors, showSensors); - sensors[i]->showCoordinateSystem(showSensors); + sensor->setupVisualization(showSensors, showSensors); + sensor->showCoordinateSystem(showSensors); } // rebuild visualization @@ -547,9 +547,9 @@ void showRobotWindow::updateJointBox() { UI.comboBoxJoint->clear(); - for (unsigned int i = 0; i < currentRobotNodes.size(); i++) + for (auto & currentRobotNode : currentRobotNodes) { - UI.comboBoxJoint->addItem(QString(currentRobotNodes[i]->getName().c_str())); + UI.comboBoxJoint->addItem(QString(currentRobotNode->getName().c_str())); } } @@ -558,9 +558,9 @@ void showRobotWindow::updateRNSBox() UI.comboBoxRobotNodeSet->clear(); UI.comboBoxRobotNodeSet->addItem(QString("<All>")); - for (unsigned int i = 0; i < robotNodeSets.size(); i++) + for (auto & robotNodeSet : robotNodeSets) { - UI.comboBoxRobotNodeSet->addItem(QString(robotNodeSets[i]->getName().c_str())); + UI.comboBoxRobotNodeSet->addItem(QString(robotNodeSet->getName().c_str())); } } @@ -845,7 +845,7 @@ void showRobotWindow::loadRobot() { QFileInfo fileInfo(m_sRobotFilename.c_str()); std::string suffix(fileInfo.suffix().toLatin1()); - RobotImporterFactoryPtr importer = RobotImporterFactory::fromFileExtension(suffix, NULL); + RobotImporterFactoryPtr importer = RobotImporterFactory::fromFileExtension(suffix, nullptr); if (!importer) { @@ -1075,9 +1075,9 @@ void showRobotWindow::selectEEF(int nr) std::vector<std::string> ps = currentEEF->getPreshapes(); UI.comboBoxEndEffectorPS->addItem(QString("none")); - for (unsigned int i = 0; i < ps.size(); i++) + for (auto & p : ps) { - UI.comboBoxEndEffectorPS->addItem(QString(ps[i].c_str())); + UI.comboBoxEndEffectorPS->addItem(QString(p.c_str())); } } @@ -1105,8 +1105,8 @@ void showRobotWindow::updateEEFBox() { UI.comboBoxEndEffector->clear(); - for (unsigned int i = 0; i < eefs.size(); i++) + for (auto & eef : eefs) { - UI.comboBoxEndEffector->addItem(QString(eefs[i]->getName().c_str())); + UI.comboBoxEndEffector->addItem(QString(eef->getName().c_str())); } } diff --git a/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp b/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp index 253805552500280ca133ddbeecbde232931e8776..354ad20cb7d7425faf949a8386d73272ff2f15a1 100644 --- a/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp +++ b/VirtualRobot/examples/SceneViewer/showSceneWindow.cpp @@ -10,7 +10,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -26,7 +26,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; showSceneWindow::showSceneWindow(std::string& sSceneFile) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -172,7 +172,7 @@ void showSceneWindow::buildVisu() } visualization = scene->getVisualization<CoinVisualization>(visuType); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -405,9 +405,9 @@ void showSceneWindow::selectRobot(int nr) std::vector<VirtualRobot::RobotConfigPtr> roc = scene->getRobotConfigs(currentRobot); - for (size_t i = 0; i < roc.size(); i++) + for (auto & i : roc) { - QString rn = roc[i]->getName().c_str(); + QString rn = i->getName().c_str(); UI.comboBoxRobotConfig->addItem(rn); } @@ -418,9 +418,9 @@ void showSceneWindow::selectRobot(int nr) std::vector<VirtualRobot::TrajectoryPtr> tr = scene->getTrajectories(currentRobot->getName()); - for (size_t i = 0; i < tr.size(); i++) + for (auto & i : tr) { - QString rn = tr[i]->getName().c_str(); + QString rn = i->getName().c_str(); UI.comboBoxTrajectory->addItem(rn); } @@ -432,9 +432,9 @@ void showSceneWindow::selectRobot(int nr) std::vector<VirtualRobot::EndEffectorPtr> eefs = currentRobot->getEndEffectors(); - for (size_t i = 0; i < eefs.size(); i++) + for (auto & eef : eefs) { - QString rn = eefs[i]->getName().c_str(); + QString rn = eef->getName().c_str(); UI.comboBoxEEF->addItem(rn); } @@ -549,25 +549,25 @@ void showSceneWindow::updateGui() std::vector<VirtualRobot::RobotPtr> robs = scene->getRobots(); - for (size_t i = 0; i < robs.size(); i++) + for (auto & rob : robs) { - QString rn = robs[i]->getName().c_str(); + QString rn = rob->getName().c_str(); UI.comboBoxRobot->addItem(rn); } std::vector<VirtualRobot::ManipulationObjectPtr> mos = scene->getManipulationObjects(); - for (size_t i = 0; i < mos.size(); i++) + for (auto & mo : mos) { - QString mn = mos[i]->getName().c_str(); + QString mn = mo->getName().c_str(); UI.comboBoxObject->addItem(mn); } std::vector<VirtualRobot::ObstaclePtr> obs = scene->getObstacles(); - for (size_t i = 0; i < obs.size(); i++) + for (auto & ob : obs) { - QString on = obs[i]->getName().c_str(); + QString on = ob->getName().c_str(); UI.comboBoxObject->addItem(on); } diff --git a/VirtualRobot/examples/Simox2Mjcf/CMakeLists.txt b/VirtualRobot/examples/Simox2Mjcf/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..c99d34c8295b0db7bbdbcb66dc2340b0298c37a9 --- /dev/null +++ b/VirtualRobot/examples/Simox2Mjcf/CMakeLists.txt @@ -0,0 +1,50 @@ +project(Simox2Mjcf) + +#find_package(TinyXML2 7.0.0) +find_package(Boost COMPONENTS system filesystem REQUIRED) + +#if (TINYXML2_FOUND AND Boost_FOUND) +if (Boost_FOUND) + + #include_directories(${TINYXML2_INCLUDE_DIR}) + + set(SOURCES + main.cpp + ) + + set(HEADERS + ) + + set(LIBS + VirtualRobot + # ${TINYXML2_LIBRARIES} + ${BOOST_FILESYSTEM_LIBRARIES} + ${BOOST_SYSTEM_LIBRARIES} + ) + + + add_executable(${PROJECT_NAME} ${SOURCES} ${HEADERS}) + set_target_properties(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR}) + set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "Examples") + + # against undefined reference to boost::filesystem::detail::copy_file + # source: https://stackoverflow.com/a/3500721 + target_compile_definitions(${PROJECT_NAME} PRIVATE -DBOOST_NO_CXX11_SCOPED_ENUMS) + + target_link_libraries(${PROJECT_NAME} ${LIBS}) + + + ####################################################################################### + ############################ Setup for installation ################################### + ####################################################################################### + + install(TARGETS ${PROJECT_NAME} + # IMPORTANT: Add the library to the "export-set" + EXPORT SimoxTargets + RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin + COMPONENT dev) + + message( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR}) + message( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " ${INSTALL_BIN_DIR}) + +endif() diff --git a/VirtualRobot/examples/Simox2Mjcf/main.cpp b/VirtualRobot/examples/Simox2Mjcf/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e3fa1d1e819829a4890ff95a56e7ab1c489dddb5 --- /dev/null +++ b/VirtualRobot/examples/Simox2Mjcf/main.cpp @@ -0,0 +1,118 @@ +#include <boost/filesystem.hpp> + +#include <VirtualRobot/RuntimeEnvironment.h> + +#include <VirtualRobot/XML/RobotIO.h> +#include <VirtualRobot/XML/mjcf/MujocoIO.h> + + +using namespace VirtualRobot; +using VirtualRobot::RuntimeEnvironment; + +namespace fs = boost::filesystem; + + +void printUsage(const char* argv0) +{ + std::cout << "Usage: " << argv0 + << " --robot <simox robot file> " + << "[--outdir <output directory>] " + << "[--meshRelDir <relative mesh directory>] " + << "[--actuator {motor|position|velocity}] " + << "[--mocap {y|n}]" + << std::endl; +} + +/** + * Loads a Simox robot and converts it to Mujoco's XML format (MJCF). + * The converted file is stored in a directory mjcf/ placed in the directory + * of the input file. + */ +int main(int argc, char* argv[]) +{ + RuntimeEnvironment::considerKey("robot"); + RuntimeEnvironment::considerKey("outdir"); + RuntimeEnvironment::considerKey("meshRelDir"); + RuntimeEnvironment::considerKey("actuator"); + RuntimeEnvironment::considerKey("mocap"); + + RuntimeEnvironment::processCommandLine(argc, argv); + + //RuntimeEnvironment::print(); + + fs::path inputFilename; + + + if (RuntimeEnvironment::hasValue("robot")) + { + std::string robFile = RuntimeEnvironment::getValue("robot"); + + if (RuntimeEnvironment::getDataFileAbsolute(robFile)) + { + inputFilename = robFile; + } + else + { + std::cout << "Something is wrong with " << robFile; + } + } + else + { + printUsage(argv[0]); + return 0; + } + + fs::path outputDir = RuntimeEnvironment::checkParameter( + "outdir", (inputFilename.parent_path() / "mjcf").string()); + std::string meshRelDir = RuntimeEnvironment::checkParameter("meshRelDir", "mesh"); + + + std::string actuatorTypeStr = RuntimeEnvironment::checkParameter("actuator", "motor"); + mjcf::ActuatorType actuatorType; + try + { + actuatorType = mjcf::toActuatorType(actuatorTypeStr); + } + catch (const std::out_of_range&) + { + std::cout << "No actuator '" << actuatorTypeStr << "'" << std::endl; + std::cout << "Avaliable: motor|position|velocity" << std::endl; + return -1; + } + + bool mocap = RuntimeEnvironment::checkParameter("mocap", "n") == "y"; + + + std::cout << "Input file: " << inputFilename << std::endl; + std::cout << "Output dir: " << outputDir << std::endl; + std::cout << "Output mesh dir: " << outputDir / meshRelDir << std::endl; + //std::cout << "Actuator type: " << actuatorType << std::endl; + std::cout << "With mocap body: " << (mocap ? "yes" : "no "); + + std::cout << "Loading robot ..." << std::endl; + + RobotPtr robot; + try + { + robot = RobotIO::loadRobot(inputFilename.string(), RobotIO::eFull); + assert(robot); + } + catch (const VirtualRobotException&) + { + throw; // rethrow + } + + if (false) + { + // using RobotIO + RobotIO::saveMJCF(robot, inputFilename.filename().string(), outputDir.string(), meshRelDir); + } + else + { + // direct API + mjcf::MujocoIO mujocoIO(robot); + mujocoIO.setActuatorType(actuatorType); + mujocoIO.setWithMocapBody(mocap); + mujocoIO.saveMJCF(inputFilename.filename().string(), outputDir.string(), meshRelDir); + } +} diff --git a/VirtualRobot/examples/reachability/reachabilityScene.cpp b/VirtualRobot/examples/reachability/reachabilityScene.cpp index 7032899dc3b78cde648cac3770d0f95a7d9505d6..cf4814a292070f1abb89e2440d72a9e75dfac7d7 100644 --- a/VirtualRobot/examples/reachability/reachabilityScene.cpp +++ b/VirtualRobot/examples/reachability/reachabilityScene.cpp @@ -103,7 +103,7 @@ void endlessExtend(std::string robotFile, std::string reachFile, int steps) reachSpace->print(); - time_t time_now = time(NULL); + time_t time_now = time(nullptr); struct tm* timeinfo; timeinfo = localtime(&time_now); char buffer [255]; diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp index ad073a4f41b30ab988fbc47e079043a56b8cd195..316be2988c44e7013916be2e8fe6e63c5e90cb3c 100644 --- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp +++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp @@ -11,7 +11,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -31,7 +31,7 @@ float TIMER_MS = 30.0f; reachabilityWindow::reachabilityWindow(std::string& sRobotFile, std::string& reachFile, Eigen::Vector3f& axisTCP) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -239,7 +239,7 @@ void reachabilityWindow::buildVisu() SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full; visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -272,9 +272,9 @@ void reachabilityWindow::updateRNSBox() UI.comboBoxRNS->clear(); //UI.comboBoxRNS->addItem(QString("<All>")); - for (unsigned int i = 0; i < robotNodeSets.size(); i++) + for (auto & robotNodeSet : robotNodeSets) { - UI.comboBoxRNS->addItem(QString(robotNodeSets[i]->getName().c_str())); + UI.comboBoxRNS->addItem(QString(robotNodeSet->getName().c_str())); } selectRNS(0); @@ -314,9 +314,9 @@ void reachabilityWindow::updateJointBox() { UI.comboBoxJoint->clear(); - for (unsigned int i = 0; i < allRobotNodes.size(); i++) + for (auto & allRobotNode : allRobotNodes) { - UI.comboBoxJoint->addItem(QString(allRobotNodes[i]->getName().c_str())); + UI.comboBoxJoint->addItem(QString(allRobotNode->getName().c_str())); } selectJoint(0); @@ -427,16 +427,16 @@ void reachabilityWindow::loadRobot() robot->getRobotNodeSets(allRNS); robotNodeSets.clear(); - for (size_t i = 0; i < allRNS.size(); i++) + for (auto & i : allRNS) { - if (allRNS[i]->isKinematicChain()) + if (i->isKinematicChain()) { - VR_INFO << " RNS <" << allRNS[i]->getName() << "> is a valid kinematic chain" << endl; - robotNodeSets.push_back(allRNS[i]); + VR_INFO << " RNS <" << i->getName() << "> is a valid kinematic chain" << endl; + robotNodeSets.push_back(i); } else { - VR_INFO << " RNS <" << allRNS[i]->getName() << "> is not a valid kinematic chain" << endl; + VR_INFO << " RNS <" << i->getName() << "> is not a valid kinematic chain" << endl; } } @@ -514,10 +514,10 @@ void reachabilityWindow::createReach() std::vector < VirtualRobot::RobotNodeSetPtr > allRNS; robot->getRobotNodeSets(allRNS); - for (size_t i = 0; i < allRNS.size(); i++) + for (auto & i : allRNS) { - UICreate.comboBoxColModelDynamic->addItem(QString(allRNS[i]->getName().c_str())); - UICreate.comboBoxColModelStatic->addItem(QString(allRNS[i]->getName().c_str())); + UICreate.comboBoxColModelDynamic->addItem(QString(i->getName().c_str())); + UICreate.comboBoxColModelStatic->addItem(QString(i->getName().c_str())); } UICreate.comboBoxQualityMeasure->addItem(QString("Reachability")); diff --git a/VirtualRobot/examples/stability/stabilityWindow.cpp b/VirtualRobot/examples/stability/stabilityWindow.cpp index 7559c1166f01f118bce5cc579f33a533098848ff..45bec74390f58b9c5b46911361eb529bc75c334d 100644 --- a/VirtualRobot/examples/stability/stabilityWindow.cpp +++ b/VirtualRobot/examples/stability/stabilityWindow.cpp @@ -17,7 +17,7 @@ #include <QFileDialog> #include <Eigen/Geometry> -#include <time.h> +#include <ctime> #include <vector> #include <iostream> #include <cmath> @@ -37,7 +37,7 @@ using namespace VirtualRobot; float TIMER_MS = 30.0f; stabilityWindow::stabilityWindow(std::string& sRobotFile) - : QMainWindow(NULL) + : QMainWindow(nullptr) { VR_INFO << " start " << endl; @@ -202,7 +202,7 @@ void stabilityWindow::buildVisu() useColModel = UI.checkBoxColModel->checkState() == Qt::Checked; SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full; visualization = robot->getVisualization<CoinVisualization>(colModel); - SoNode* visualisationNode = NULL; + SoNode* visualisationNode = nullptr; if (visualization) { @@ -349,10 +349,10 @@ void stabilityWindow::updateSupportVisu() std::vector< Eigen::Vector2f > points2D; //MathTools::Plane plane2(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,1.0f,0)); - for (size_t u = 0; u < points.size(); u++) + for (auto & point : points) { - Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(points[u].p, p); + Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(point.p, p); points2D.push_back(pt2d); } @@ -371,13 +371,13 @@ void stabilityWindow::updateRNSBox() robot->getRobotNodeSets(allRNS); robotNodeSets.clear(); - for (size_t i = 0; i < allRNS.size(); i++) + for (auto & i : allRNS) { //if (allRNS[i]->isKinematicChain()) //{ //VR_INFO << " RNS <" << allRNS[i]->getName() << "> is a valid kinematic chain" << endl; - robotNodeSets.push_back(allRNS[i]); - UI.comboBoxRNS->addItem(QString(allRNS[i]->getName().c_str())); + robotNodeSets.push_back(i); + UI.comboBoxRNS->addItem(QString(i->getName().c_str())); /*} else { VR_INFO << " RNS <" << allRNS[i]->getName() << "> is not a valid kinematic chain" << endl; @@ -385,9 +385,9 @@ void stabilityWindow::updateRNSBox() } - for (unsigned int i = 0; i < allRobotNodes.size(); i++) + for (auto & allRobotNode : allRobotNodes) { - allRobotNodes[i]->showBoundingBox(false); + allRobotNode->showBoundingBox(false); } @@ -447,9 +447,9 @@ void stabilityWindow::updateJointBox() { UI.comboBoxJoint->clear(); - for (unsigned int i = 0; i < currentRobotNodes.size(); i++) + for (auto & currentRobotNode : currentRobotNodes) { - UI.comboBoxJoint->addItem(QString(currentRobotNodes[i]->getName().c_str())); + UI.comboBoxJoint->addItem(QString(currentRobotNode->getName().c_str())); } selectJoint(0); diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp index 66c19effdc8f39b3e81815eff7f695b56b253a9e..242b85010ec48971af9918393d9cfc61dd11978b 100644 --- a/VirtualRobot/math/Helpers.cpp +++ b/VirtualRobot/math/Helpers.cpp @@ -25,13 +25,16 @@ using namespace math; -Eigen::Vector3f math::Helpers::GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f &dir1, Eigen::Vector3f &dir2) +#define M_PI_F (static_cast<float>(M_PI)) + +Eigen::Vector3f math::Helpers::GetOrthonormalVectors( + Eigen::Vector3f vec, Eigen::Vector3f &dir1, Eigen::Vector3f &dir2) { vec = vec.normalized(); - dir1 = vec.cross(Eigen::Vector3f(0,0,1)); + dir1 = vec.cross(Eigen::Vector3f::UnitZ()); if (dir1.norm() < 0.1f) { - dir1 = vec.cross(Eigen::Vector3f(0,1,0)); + dir1 = vec.cross(Eigen::Vector3f::UnitY()); } dir1 = -dir1.normalized(); dir2 = vec.cross(dir1).normalized(); @@ -41,13 +44,14 @@ Eigen::Vector3f math::Helpers::GetOrthonormalVectors(Eigen::Vector3f vec, Eigen: float Helpers::ShiftAngle0_2PI(float a) { while (a < 0) a += 2*M_PI; - while (a > 2*M_PI) a -= 2*M_PI; + while (a > 2 * M_PI_F) + a -= 2*M_PI; return a; } float Helpers::AngleModPI(float value) { - return ShiftAngle0_2PI(value + M_PI) - M_PI; + return ShiftAngle0_2PI(value + M_PI_F) - M_PI_F; } void Helpers::GetIndex(float t, float minT, float maxT, int count, int &i, float &f) @@ -58,7 +62,7 @@ void Helpers::GetIndex(float t, float minT, float maxT, int count, int &i, float return; } f = ((t - minT) / (maxT - minT)) * (count - 1); - i = std::max(0, std::min(count - 2, (int)f)); + i = std::max(0, std::min(count - 2, static_cast<int>(f))); f -= i; } @@ -99,12 +103,16 @@ float Helpers::ILerp(float a, float b, float f) float Helpers::Lerp(float a, float b, int min, int max, int val) { if (min == max) return a; //TODO - return Lerp(a, b, (val - min) / (float)(max - min)); + return Lerp(a, b, (val - min) / static_cast<float>(max - min)); } -float Helpers::Angle(Eigen::Vector2f v) { return (float)std::atan2(v.y(), v.x());} +float Helpers::Angle(Eigen::Vector2f v) +{ + return static_cast<float>(std::atan2(v.y(), v.x())); +} -int Helpers::Sign(float x){ +int Helpers::Sign(float x) +{ return x > 0 ? 1 : (x == 0 ? 0 : -1); } @@ -118,7 +126,7 @@ std::vector<float> Helpers::FloatRange(float start, float end, int steps) { std::vector<float> result; for (int i = 0; i < steps+1; ++i) { - result.push_back(Lerp(start, end, i/(float) steps)); + result.push_back(Lerp(start, end, i/static_cast<float>(steps))); } return result; } @@ -147,129 +155,119 @@ std::vector<Eigen::Vector3f> Helpers::VectorRange(std::vector<float> xvals, std: float Helpers::SmallestAngle(Eigen::Vector3f a, Eigen::Vector3f b) { - return (float)std::atan2(a.cross(b).norm(), a.dot(b)); + return static_cast<float>(std::atan2(a.cross(b).norm(), a.dot(b))); } -Eigen::Vector3f Helpers::CwiseMin(Eigen::Vector3f a, Eigen::Vector3f b) +Eigen::Vector3f Helpers::CwiseMin(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { - return Eigen::Vector3f(std::min(a.x(), b.x()), - std::min(a.y(), b.y()), - std::min(a.z(), b.z())); + return a.cwiseMin(b); } -Eigen::Vector3f Helpers::CwiseMax(Eigen::Vector3f a, Eigen::Vector3f b) +Eigen::Vector3f Helpers::CwiseMax(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { - return Eigen::Vector3f(std::max(a.x(), b.x()), - std::max(a.y(), b.y()), - std::max(a.z(), b.z())); - + return a.cwiseMax(b); } -Eigen::Vector3f Helpers::CwiseDivide(Eigen::Vector3f a, Eigen::Vector3f b) +Eigen::Vector3f Helpers::CwiseDivide(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { - return Eigen::Vector3f(a.x()/ b.x(), - a.y()/ b.y(), - a.z()/ b.z()); + return a.cwiseQuotient(b); } -Eigen::Vector3f Helpers::Average(std::vector<Eigen::Vector3f> vectors) +Eigen::Vector3f Helpers::Average(const std::vector<Eigen::Vector3f>& vectors) { - Eigen::Vector3f avg = Eigen::Vector3f(0,0,0); - if(vectors.size() == 0) return avg; - for(Eigen::Vector3f v : vectors){ + Eigen::Vector3f avg = Eigen::Vector3f::Zero(); + if (vectors.size() == 0) return avg; + for (const Eigen::Vector3f& v : vectors) + { avg += v; } avg /= vectors.size(); return avg; } -void Helpers::Swap(float &a,float &b) + +void Helpers::Swap(float& a,float& b) { - float t = a; - a = b; - b = t; + std::swap(a, b); } -Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f &pos, const Eigen::Quaternionf &ori) +Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori) { - Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); - pose.block<3,3>(0,0) = ori.toRotationMatrix(); - pose.block<3,1>(0,3) = pos; - return pose; + return Pose(pos, ori); } -Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f &pos, const Eigen::Matrix3f &ori) +Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f& pos, const Eigen::Matrix3f& ori) { - Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); - pose.block<3,3>(0,0) = ori; - pose.block<3,1>(0,3) = pos; - return pose; + return Pose(pos, ori); } +Eigen::Vector3f Helpers::GetPosition(const Eigen::Matrix4f& pose) +{ + return Position(pose); +} -Eigen::Matrix3f Helpers::GetRotationMatrix(const Eigen::Vector3f & source, const Eigen::Vector3f & target) +Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose) { - Eigen::Vector3f src = source.normalized(); - Eigen::Vector3f tgt = target.normalized(); - float angle = acos(src.dot(tgt)); - if(fabs(angle) < 0.001f) - { - return Eigen::Matrix3f::Identity(); - } - Eigen::Vector3f axis = src.cross(tgt); - axis.normalize(); - return Eigen::AngleAxisf(angle, axis).toRotationMatrix(); + return Orientation(pose); } -Eigen::Matrix3f Helpers::RotateOrientationToFitVector(const Eigen::Matrix3f &ori, const Eigen::Vector3f &localSource, const Eigen::Vector3f &globalTarget) + +Eigen::Matrix4f math::Helpers::TranslatePose(const Eigen::Matrix4f& pose, const Eigen::Vector3f& offset) { - Eigen::Vector3f vec = ori * localSource; - return GetRotationMatrix(vec, globalTarget) * ori; + Eigen::Matrix4f p = pose; + Position(p) += offset; + return p; } -Eigen::Vector3f Helpers::CreateVectorFromCylinderCoords(float r, float angle, float z) +void Helpers::InvertPose(Eigen::Matrix4f& pose) { - return Eigen::Vector3f(r * cos(angle), r * sin(angle), z); + Orientation(pose).transposeInPlace(); + Position(pose) = - Orientation(pose) * Position(pose); } -Eigen::Matrix4f math::Helpers::TranslatePose(const Eigen::Matrix4f &pose, const Eigen::Vector3f &offset) + +Eigen::Matrix3f Helpers::GetRotationMatrix(const Eigen::Vector3f& source, const Eigen::Vector3f& target) { - Eigen::Matrix4f p = pose; - p.block<3, 1>(0, 3) += offset; - return p; + // no normalization needed + return Eigen::Quaternionf::FromTwoVectors(source, target).toRotationMatrix(); } -Eigen::Vector3f Helpers::TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f& pos) +Eigen::Vector3f Helpers::CreateVectorFromCylinderCoords(float r, float angle, float z) { - return (transform * Eigen::Vector4f(pos(0), pos(1), pos(2), 1)).block<3,1>(0,0); + return Eigen::Vector3f(r * std::cos(angle), r * std::sin(angle), z); } -Eigen::Vector3f Helpers::TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f& dir) +Eigen::Matrix3f Helpers::RotateOrientationToFitVector( + const Eigen::Matrix3f& ori, const Eigen::Vector3f& localSource, + const Eigen::Vector3f& globalTarget) { - return transform.block<3,3>(0,0) * dir; + Eigen::Vector3f vec = ori * localSource; + return GetRotationMatrix(vec, globalTarget) * ori; } -Eigen::Matrix3f Helpers::TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f& ori) + +Eigen::Vector3f Helpers::TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f& pos) { - return transform.block<3,3>(0,0) * ori; + return (transform * pos.homogeneous()).head<3>(); } -float Helpers::Distance(const Eigen::Matrix4f &a, const Eigen::Matrix4f &b, float rad2mmFactor) +Eigen::Vector3f Helpers::TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f& dir) { - Eigen::AngleAxisf aa(b.block<3,3>(0,0) * a.block<3,3>(0,0).inverse()); - float dist = (a.block<3, 1>(0, 3) - b.block<3, 1>(0, 3)).norm(); - return dist + AngleModPI(aa.angle()) * rad2mmFactor; + return Orientation(transform) * dir; } -Eigen::Vector3f Helpers::GetPosition(const Eigen::Matrix4f& pose) +Eigen::Matrix3f Helpers::TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f& ori) { - return pose.block<3, 1>(0, 3); + return Orientation(transform) * ori; } -Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose) +float Helpers::Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor) { - return pose.block<3, 3>(0, 0); + Eigen::AngleAxisf aa(Orientation(b) * Orientation(a).inverse()); + float dist = (Position(a) - Position(b)).norm(); + return dist + AngleModPI(aa.angle()) * rad2mmFactor; } + Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen) { if(maxLen.rows() != 1 && maxLen.rows() != vec.rows()) @@ -290,10 +288,10 @@ Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eig float Helpers::rad2deg(float rad) { - return rad * (float)(180.0 / M_PI); + return rad * 180.0f / M_PI_F; } float Helpers::deg2rad(float deg) { - return deg * (float)(M_PI / 180.0); + return deg * M_PI_F / 180.0f; } diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h index 10e6b9e62c31f3883f04d3d9f77d0f528f46a05e..0755446e95204250615e943f76b82826afa19227 100644 --- a/VirtualRobot/math/Helpers.h +++ b/VirtualRobot/math/Helpers.h @@ -48,29 +48,169 @@ namespace math static std::vector<Eigen::Vector3f> VectorRangeSymmetric(float start, float end, int steps); static std::vector<Eigen::Vector3f> VectorRange(std::vector<float> xvals, std::vector<float> yvals, std::vector<float> zvals); static float SmallestAngle(Eigen::Vector3f a, Eigen::Vector3f b); - static Eigen::Vector3f CwiseMin(Eigen::Vector3f a, Eigen::Vector3f b); - static Eigen::Vector3f CwiseMax(Eigen::Vector3f a, Eigen::Vector3f b); - static Eigen::Vector3f CwiseDivide(Eigen::Vector3f a, Eigen::Vector3f b); - static Eigen::Vector3f Average(std::vector<Eigen::Vector3f> vectors); - static void Swap(float &a,float &b); + + static Eigen::Vector3f CwiseMin(const Eigen::Vector3f& a, const Eigen::Vector3f& b); + static Eigen::Vector3f CwiseMax(const Eigen::Vector3f& a, const Eigen::Vector3f& b); + static Eigen::Vector3f CwiseDivide(const Eigen::Vector3f& a, const Eigen::Vector3f& b); + static Eigen::Vector3f Average(const std::vector<Eigen::Vector3f>& vectors); + static void Swap(float& a,float& b); + + + // POSE UTILITY + + /// Get the position block from the given pose. + template <typename Derived> + static Eigen::Block<Derived, 3, 1> + Position(Eigen::MatrixBase<Derived>& pose); + + /// Get the position block from the given pose. + template <typename Derived> + static const Eigen::Block<const Derived, 3, 1> + Position(const Eigen::MatrixBase<Derived>& pose); + + + /// Get the orientation block from the given pose. + template <typename Derived> + static Eigen::Block<Derived, 3, 3> + Orientation(Eigen::MatrixBase<Derived>& pose); + + /// Get the orientation block from the given pose. + template <typename Derived> + static const Eigen::Block<const Derived, 3, 3> + Orientation(const Eigen::MatrixBase<Derived>& pose); + + + /// Build a pose matrix from the given position and orientation. + template <typename PosDerived, typename OriDerived> + static Eigen::Matrix4f + Pose(const Eigen::MatrixBase<PosDerived>& pos, const Eigen::MatrixBase<OriDerived>& ori); + + /// Build a pose matrix from the given position and orientation. + template <typename PosDerived, typename OriDerived> + static Eigen::Matrix4f + Pose(const Eigen::MatrixBase<PosDerived>& pos, const Eigen::RotationBase<OriDerived, 3>& ori); + + + /// Legacy shortcut for Pose(). static Eigen::Matrix4f CreatePose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori); + /// Legacy shortcut for Pose(). static Eigen::Matrix4f CreatePose(const Eigen::Vector3f& pos, const Eigen::Matrix3f& ori); - static Eigen::Matrix3f GetRotationMatrix(const Eigen::Vector3f &source, const Eigen::Vector3f &target); - static Eigen::Matrix3f RotateOrientationToFitVector(const Eigen::Matrix3f& ori, const Eigen::Vector3f &localSource, const Eigen::Vector3f &globalTarget); - static Eigen::Vector3f CreateVectorFromCylinderCoords(float r, float angle, float z); - static Eigen::Matrix4f TranslatePose(const Eigen::Matrix4f &pose, const Eigen::Vector3f& offset); - static Eigen::Vector3f TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f &pos); - static Eigen::Vector3f TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f &dir); - static Eigen::Matrix3f TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f &ori); - static float Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor); + + /// Legacy shortcut for Position() as getter. static Eigen::Vector3f GetPosition(const Eigen::Matrix4f& pose); + /// Legacy shortcut for Orientation() as getter. static Eigen::Matrix3f GetOrientation(const Eigen::Matrix4f& pose); - static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen); + + /// Translate the given pose by the given offset. + static Eigen::Matrix4f TranslatePose(const Eigen::Matrix4f& pose, const Eigen::Vector3f& offset); + + /// Invert the given pose in-place. + static void InvertPose(Eigen::Matrix4f& pose); + /// Return the inverted of the given pose. + template <typename Derived> + static Eigen::Matrix4f InvertedPose(const Eigen::MatrixBase<Derived>& pose); + + /// Get a vector from cylinder coordinates. + static Eigen::Vector3f CreateVectorFromCylinderCoords(float r, float angle, float z); + + /// Get a rotation matrix rotating source to target. + static Eigen::Matrix3f GetRotationMatrix(const Eigen::Vector3f& source, const Eigen::Vector3f& target); + + static Eigen::Matrix3f RotateOrientationToFitVector( + const Eigen::Matrix3f& ori, const Eigen::Vector3f& localSource, const Eigen::Vector3f& globalTarget); + + + /// Transform the position by the transform. + static Eigen::Vector3f TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f& pos); + /// Transform the direction by the transform. + static Eigen::Vector3f TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f& dir); + /// Transform the orientation by the transform. + static Eigen::Matrix3f TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f& ori); + + static float Distance(const Eigen::Matrix4f& a, const Eigen::Matrix4f& b, float rad2mmFactor); + static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen); + + static float rad2deg(float rad); static float deg2rad(float deg); - + + private: + }; + + + + template <typename Derived> + Eigen::Block<Derived, 3, 1> Helpers::Position(Eigen::MatrixBase<Derived>& pose) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4, 4); + return Eigen::Block<Derived, 3, 1>(pose.derived(), 0, 3); + } + + template <typename Derived> + const Eigen::Block<const Derived, 3, 1> Helpers::Position(const Eigen::MatrixBase<Derived>& pose) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4, 4); + return Eigen::Block<const Derived, 3, 1>(pose.derived(), 0, 3); + } + + + template <typename Derived> + Eigen::Block<Derived, 3, 3> Helpers::Orientation(Eigen::MatrixBase<Derived>& pose) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4, 4); + return Eigen::Block<Derived, 3, 3>(pose.derived(), 0, 0); + } + + template <typename Derived> + const Eigen::Block<const Derived, 3, 3> Helpers::Orientation(const Eigen::MatrixBase<Derived>& pose) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4, 4); + return Eigen::Block<const Derived, 3, 3>(pose.derived(), 0, 0); + } + + + template <typename PosDerived, typename OriDerived> + Eigen::Matrix4f Helpers::Pose(const Eigen::MatrixBase<PosDerived>& pos, + const Eigen::MatrixBase<OriDerived>& ori) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<PosDerived>, 3, 1); + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<OriDerived>, 3, 3); + Eigen::Matrix4f pose = pose.Identity(); + Position(pose) = pos; + Orientation(pose) = ori; + return pose; + } + + template <typename PosDerived, typename OriDerived> + Eigen::Matrix4f Helpers::Pose(const Eigen::MatrixBase<PosDerived>& pos, + const Eigen::RotationBase<OriDerived, 3>& ori) + { + return Pose(pos, ori.toRotationMatrix()); + } + + + template <typename Derived> + Eigen::Matrix4f Helpers::InvertedPose(const Eigen::MatrixBase<Derived>& pose) + { + Eigen::Matrix4f inv = pose; + InvertPose(inv); + return inv; + } + +} + + +namespace Eigen +{ + template <typename Derived> + std::ostream& operator<< (std::ostream& os, const QuaternionBase<Derived>& quat) + { + os << "[ " << quat.w() << " | " + << quat.x() << " " << quat.y() << " " << quat.z() << " ]"; + return os; + } } diff --git a/VirtualRobot/math/Line.cpp b/VirtualRobot/math/Line.cpp index fcbc168f0b3ded5b61960dd68957cf55bfa15aca..ddc2519a28113ddd2f646a5e618293c0ff6ace3d 100644 --- a/VirtualRobot/math/Line.cpp +++ b/VirtualRobot/math/Line.cpp @@ -34,7 +34,7 @@ Line::Line(Eigen::Vector3f pos, Eigen::Vector3f dir) { } -Line Line::Normalized() +Line Line::Normalized() const { return Line(pos, dir.normalized()); } @@ -49,18 +49,26 @@ Eigen::Vector3f Line::GetDerivative(float) return dir; } -Eigen::Vector3f Line::GetClosestPoint(Eigen::Vector3f p) +Eigen::Vector3f Line::GetClosestPoint(Eigen::Vector3f p) const { return pos - (pos - p).dot(dir) * dir / dir.squaredNorm(); } -float Line::GetT(Eigen::Vector3f p) +Eigen::Vector3f Line::GetDistanceToPoint(Eigen::Vector3f p) const +{ + const Eigen::Vector3f n = dir.normalized(); + const Eigen::Vector3f pInLine = (p - pos); + const float len = (n.transpose() * pInLine); + return pInLine - n * len; +} + +float Line::GetT(Eigen::Vector3f p) const { return (p - pos).dot(dir) / dir.squaredNorm(); } -std::string Line::ToString() +std::string Line::ToString() const { std::stringstream ss; ss << "(" << pos << ") (" << dir << ")"; @@ -69,7 +77,7 @@ std::string Line::ToString() //https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm -bool Line::IntersectsTriangle(Triangle tri, float &out) +bool Line::IntersectsTriangle(Triangle tri, float &out) const { const float EPS = 0.000; //TODO Eigen::Vector3f e1, e2; //Edge1, Edge2 @@ -119,7 +127,7 @@ bool Line::IntersectsTriangle(Triangle tri, float &out) return 0; } -bool Line::IntersectsPrimitive(PrimitivePtr p, float &out) +bool Line::IntersectsPrimitive(PrimitivePtr p, float &out) const { float min = FLT_MAX; float t; @@ -142,7 +150,7 @@ Line Line::FromPoses(const Eigen::Matrix4f &p1, const Eigen::Matrix4f &p2) return FromPoints(p1.block<3,1>(0,3), p2.block<3,1>(0,3)); } -Line Line::Transform(const Eigen::Matrix4f &pose) +Line Line::Transform(const Eigen::Matrix4f &pose) const { return Line(Helpers::TransformPosition(pose, pos), Helpers::TransformDirection(pose, dir)); } diff --git a/VirtualRobot/math/Line.h b/VirtualRobot/math/Line.h index 2e2560eb2b6e07c2d3af3126e535716e9276fd59..1cf860bf10eb107abf72c424a22ce6ea5363b7e7 100644 --- a/VirtualRobot/math/Line.h +++ b/VirtualRobot/math/Line.h @@ -36,20 +36,21 @@ namespace math Eigen::Vector3f Pos(){return pos;} Eigen::Vector3f Dir(){return dir;} - Line Normalized(); + Line Normalized() const; Eigen::Vector3f Get(float t) override; Eigen::Vector3f GetDerivative(float t) override; - Eigen::Vector3f GetClosestPoint(Eigen::Vector3f p); - float GetT(Eigen::Vector3f p); - std::string ToString(); + Eigen::Vector3f GetClosestPoint(Eigen::Vector3f p) const; + Eigen::Vector3f GetDistanceToPoint(Eigen::Vector3f p) const; + float GetT(Eigen::Vector3f p) const; + std::string ToString() const; - bool IntersectsTriangle(Triangle tri, float& t); - bool IntersectsPrimitive(PrimitivePtr p, float& t); + bool IntersectsTriangle(Triangle tri, float& t) const; + bool IntersectsPrimitive(PrimitivePtr p, float& t) const; static Line FromPoints(Eigen::Vector3f p1, Eigen::Vector3f p2); static Line FromPoses(const Eigen::Matrix4f& p1, const Eigen::Matrix4f& p2); - Line Transform(const Eigen::Matrix4f& pose); + Line Transform(const Eigen::Matrix4f& pose) const; private: Eigen::Vector3f pos; diff --git a/VirtualRobot/math/MarchingCubes.cpp b/VirtualRobot/math/MarchingCubes.cpp index fd36d715e939fabf8bddf6917ef1ac157a5dab24..eb3efb2fed3b5f39ab1a5fc5f4c283a22f9099b6 100644 --- a/VirtualRobot/math/MarchingCubes.cpp +++ b/VirtualRobot/math/MarchingCubes.cpp @@ -308,6 +308,8 @@ void MarchingCubes::Build(PrimitivePtr res, int tianglesNum) 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0 }; + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wnarrowing" const char MarchingCubes::_triTable[256][16] = {{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, @@ -566,5 +568,6 @@ void MarchingCubes::Build(PrimitivePtr res, int tianglesNum) {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1} }; + #pragma GCC diagnostic pop diff --git a/VirtualRobot/math/MathForwardDefinitions.h b/VirtualRobot/math/MathForwardDefinitions.h index 81910575e41fa9cb0283aa8ef06ab35c38cb769b..f5213a9af794e772da7663163cd3f8ab86652616 100644 --- a/VirtualRobot/math/MathForwardDefinitions.h +++ b/VirtualRobot/math/MathForwardDefinitions.h @@ -31,7 +31,9 @@ #include <stdexcept> #include <vector> -template<class T> class Nullable { + +template<class T> class Nullable +{ public: Nullable(T& value) : defined(true), value(value) @@ -61,7 +63,8 @@ private: -namespace math{ +namespace math +{ //typedef Eigen::Vector2f Eigen::Vector2f; @@ -132,7 +135,8 @@ namespace math{ } -namespace sim{ +namespace sim +{ class Simulation; typedef boost::shared_ptr<Simulation> SimulationPtr; class HapticExplorationData; @@ -158,7 +162,8 @@ namespace sim{ } } -namespace explorationControllers{ +namespace explorationControllers +{ class AbstractExplorationController; typedef boost::shared_ptr<AbstractExplorationController> AbstractExplorationControllerPtr; diff --git a/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp b/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp index ae106e5d42619c5b8c4891493f39f5bd12898b9a..486040a5810cee041578fa944464d4fc09295544 100644 --- a/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp +++ b/VirtualRobot/math/SimpleAbstractFunctionR1R6.cpp @@ -22,9 +22,15 @@ */ #include "SimpleAbstractFunctionR1R6.h" +#include "Helpers.h" using namespace armarx; SimpleAbstractFunctionR1R6::SimpleAbstractFunctionR1R6() { } + +Eigen::Matrix4f math::SimpleAbstractFunctionR1R6::GetPose(float t) +{ + return math::Helpers::CreatePose(GetPosition(t), GetOrientation(t)); +} diff --git a/VirtualRobot/math/SimpleAbstractFunctionR1R6.h b/VirtualRobot/math/SimpleAbstractFunctionR1R6.h index aa5f98445dec3a1f8929477ff24c9704b9b11599..eb6f3179b4d310e5184b0e718e18e7100f08974c 100644 --- a/VirtualRobot/math/SimpleAbstractFunctionR1R6.h +++ b/VirtualRobot/math/SimpleAbstractFunctionR1R6.h @@ -31,6 +31,7 @@ namespace math public: virtual Eigen::Vector3f GetPosition(float t) = 0; virtual Eigen::Quaternionf GetOrientation(float t) = 0; + Eigen::Matrix4f GetPose(float t); private: }; diff --git a/VirtualRobot/tests/CMakeLists.txt b/VirtualRobot/tests/CMakeLists.txt index 291ef6f4d7a5691e4eadfbb7cd402526d0d7fa8a..36cea17a9ec85b44d1cf7044ec1c479b9c544f20 100644 --- a/VirtualRobot/tests/CMakeLists.txt +++ b/VirtualRobot/tests/CMakeLists.txt @@ -30,6 +30,9 @@ ADD_VR_TEST( VirtualRobotMeshImportTest ) ADD_VR_TEST( VirtualRobotTimeOptimalTrajectoryTest ) +ADD_VR_TEST( VirtualRobotJsonEigenConversionTest ) + +ADD_VR_TEST( MathFitPlaneTest ) ADD_VR_TEST( MathGaussianImplicitSurface3DNormalsTest ) ADD_VR_TEST( MathGaussianImplicitSurface3DTest ) -ADD_VR_TEST( MathFitPlaneTest ) +ADD_VR_TEST( MathHelpersTest ) diff --git a/VirtualRobot/tests/MathHelpersTest.cpp b/VirtualRobot/tests/MathHelpersTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..426a34e511ca1ce4b520eb40d10c6b062d74ceb5 --- /dev/null +++ b/VirtualRobot/tests/MathHelpersTest.cpp @@ -0,0 +1,189 @@ +/** +* @package VirtualRobot +* @author Rainer Kartmann +* @copyright 2019 Rainer Kartmann +*/ + +#define BOOST_TEST_MODULE VirtualRobot_MathHelpersTest + +#include <VirtualRobot/VirtualRobotTest.h> +#include <VirtualRobot/math/Helpers.h> +#include <string> +#include <stdio.h> + + +using namespace Eigen; +using Helpers = math::Helpers; + + +BOOST_AUTO_TEST_SUITE(MathHelpers) + + +BOOST_AUTO_TEST_CASE(test_CwiseMin_CwiseMax) +{ + Eigen::Vector3f a (-1, 3, 5), b(0, 3, 1); + Eigen::Vector3f min (-1, 3, 1); + Eigen::Vector3f max (0, 3, 5); + BOOST_CHECK_EQUAL(Helpers::CwiseMin(a, b), min); + BOOST_CHECK_EQUAL(Helpers::CwiseMax(a, b), max); +} + +BOOST_AUTO_TEST_CASE(test_CwiseDivide) +{ + Eigen::Vector3f a (0, 5, -9), b(10, 2, 3); + Eigen::Vector3f quot (0, 2.5, -3); + BOOST_CHECK_EQUAL(Helpers::CwiseDivide(a, b), quot); +} + +BOOST_AUTO_TEST_CASE(test_Swap) +{ + float a = 5, b = -10; + Helpers::Swap(a, b); + BOOST_CHECK_EQUAL(a, -10); + BOOST_CHECK_EQUAL(b, 5); +} + +BOOST_AUTO_TEST_CASE(test_GetRotationMatrix) +{ + Eigen::Vector3f source(1, 2, 3), target(-3, 2, 5); // not normalized + Eigen::Matrix3f matrix = Helpers::GetRotationMatrix(source, target); + + BOOST_CHECK((matrix * matrix.transpose()).isIdentity(1e-6f)); + BOOST_CHECK((matrix * source.normalized()).isApprox(target.normalized(), 1e-6f)); +} + +BOOST_AUTO_TEST_CASE(test_TransformPosition) +{ + Eigen::Vector3f vector(1, 2, 3); + + Eigen::Vector3f translation(4, 5, 6); + Eigen::AngleAxisf rotation(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY()); + + Eigen::Matrix4f transform = transform.Identity(); + + // identity + transform.setIdentity(); + BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector), + vector); + + // translation only + transform.setIdentity(); + Helpers::Position(transform) = translation; + BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector), + vector + translation); + + // rotation only + transform.setIdentity(); + Helpers::Orientation(transform) = rotation.toRotationMatrix(); + BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector), + rotation * vector); + + // full transform + transform.setIdentity(); + Helpers::Position(transform) = translation; + Helpers::Orientation(transform) = rotation.toRotationMatrix(); + BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector), + rotation * vector + translation); +} + + +BOOST_AUTO_TEST_CASE(test_InvertPose) +{ + Eigen::Vector3f translation(4, 5, 6); + Eigen::AngleAxisf rotation(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY()); + + Eigen::Matrix4f pose = Helpers::Pose(translation, rotation); + Eigen::Matrix4f inv; + + // in-place + inv = pose; + Helpers::InvertPose(inv); + BOOST_CHECK((pose * inv).isIdentity(1e-6f)); + BOOST_CHECK((inv * pose).isIdentity(1e-6f)); + + // returned + inv.setIdentity(); + inv = Helpers::InvertedPose(pose); + BOOST_CHECK((pose * inv).isIdentity(1e-6f)); + BOOST_CHECK((inv * pose).isIdentity(1e-6f)); +} + + +BOOST_AUTO_TEST_SUITE_END() + + + +struct BlockFixture +{ + BlockFixture() + { + quat = Quaternionf{ + AngleAxisf(static_cast<float>(M_PI), Vector3f::UnitZ()) + * AngleAxisf(static_cast<float>(M_PI_2), Vector3f::UnitY()) + }; + + quat2 = AngleAxisf(static_cast<float>(M_PI_4), Vector3f::UnitX()) * quat; + + pos = Vector3f{ 1, 2, 3 }; + pos2 = Vector3f{ 4, 5, 6 }; + + ori = quat.toRotationMatrix(); + ori2 = quat2.toRotationMatrix(); + + pose.setIdentity(); + pose.block<3, 1>(0, 3) = pos; + pose.block<3, 3>(0, 0) = ori; + } + + Matrix4f pose; + Vector3f pos, pos2; + Matrix3f ori, ori2; + Quaternionf quat, quat2; +}; + + +BOOST_FIXTURE_TEST_SUITE(MathHelpers, BlockFixture) + +using namespace math; + + +BOOST_AUTO_TEST_CASE(test_posBlock_const) +{ + BOOST_CHECK_EQUAL(Helpers::Position(const_cast<const Matrix4f&>(pose)), pos); +} + +BOOST_AUTO_TEST_CASE(test_posBlock_nonconst) +{ + BOOST_CHECK_EQUAL(Helpers::Position(pose), pos); + + Helpers::Position(pose) = pos2; + BOOST_CHECK_EQUAL(Helpers::Position(pose), pos2); +} + + +BOOST_AUTO_TEST_CASE(test_oriBlock_const) +{ + BOOST_CHECK_EQUAL(Helpers::Orientation(const_cast<const Eigen::Matrix4f&>(pose)), ori); +} + +BOOST_AUTO_TEST_CASE(test_oriBlock_nonconst) +{ + BOOST_CHECK_EQUAL(Helpers::Orientation(pose), ori); + + Helpers::Orientation(pose) = ori2; + BOOST_CHECK_EQUAL(Helpers::Orientation(pose), ori2); +} + + +BOOST_AUTO_TEST_CASE(test_toPose_matrix_and_quaternion) +{ + BOOST_CHECK_EQUAL(Helpers::Pose(pos, quat), pose); +} + +BOOST_AUTO_TEST_CASE(test_toPose_matrix_and_rotation_matrix) +{ + BOOST_CHECK_EQUAL(Helpers::Pose(pos, ori), pose); +} + + +BOOST_AUTO_TEST_SUITE_END() diff --git a/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp b/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7080332e6b1ac80bb20842863da9c059ebe512e2 --- /dev/null +++ b/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp @@ -0,0 +1,88 @@ +/** +* @package VirtualRobot +* @author Rainer Kartmann +* @copyright 2018 Rainer Kartmann +*/ + +#define BOOST_TEST_MODULE VirtualRobot_VirtualRobotJsonEigenConversionTest + +#include <VirtualRobot/VirtualRobotTest.h> + +#include <Eigen/Core> +#include <Eigen/Geometry> +#include <VirtualRobot/Util/json/eigen_conversion.hpp> + + +namespace Eigen +{ + bool operator==(const Quaternionf& lhs, const Quaternionf& rhs) + { + return lhs.isApprox(rhs, 0); + } + + std::ostream& operator<<(std::ostream& os, const Quaternionf& rhs) + { + os << "[ " << rhs.w() << " | " << rhs.x() << " " << rhs.y() << " " << rhs.z() << " ]"; + return os; + } +} + + +using json = nlohmann::json; +using namespace Eigen; + + +BOOST_AUTO_TEST_SUITE(VirtualRobotJsonEigenConversionTest); + + +BOOST_AUTO_TEST_CASE(test_matrix) +{ + Matrix4f in = in.Identity(), out = out.Zero(); + + for (int i = 0; i < in.size(); ++i) + { + in(i) = i; + } + + json j; + j = in; + out = j; + BOOST_TEST_MESSAGE("JSON: \n" << j.dump(2)); + + BOOST_CHECK_EQUAL(in, out); +} + + +BOOST_AUTO_TEST_CASE(test_vector) +{ + Vector3f in = in.Identity(), out = out.Zero(); + + for (int i = 0; i < in.size(); ++i) + { + in(i) = i; + } + + json j; + j = in; + out = j; + BOOST_TEST_MESSAGE("JSON: \n" << j.dump(2)); + + BOOST_CHECK_EQUAL(in, out); +} + + +BOOST_AUTO_TEST_CASE(test_quaternion) +{ + Quaternionf in { AngleAxisf(static_cast<float>(M_PI), Vector3f(1, 1, 1).normalized()) }; + Quaternionf out = out.Identity(); + + json j; + j = in; + out = j.get<Quaternionf>(); + BOOST_TEST_MESSAGE("JSON: \n" << j.dump(2)); + + BOOST_CHECK_EQUAL(in, out); +} + + +BOOST_AUTO_TEST_SUITE_END() diff --git a/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp b/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp index 55db998f0218b69fa8bf24044ad0c83911216bb7..86eb0b82390b0c2f179d40463ce4f57ea1441507 100644 --- a/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp +++ b/VirtualRobot/tests/VirtualRobotWorkSpaceTest.cpp @@ -50,9 +50,9 @@ BOOST_AUTO_TEST_CASE(testWorkSpaceEuler) BOOST_CHECK_SMALL(x[5], 1e-6f); // identity, vector -> matrix - for (int i = 0; i < 6; i++) + for (float & i : x) { - x[i] = 0.0f; + i = 0.0f; } ws->vector2Matrix(x, m); diff --git a/config.cmake b/config.cmake index 9cb9c6f4c2dd0c80cb7da332ac87d6508e26819a..49608f6fe8fc0cc88cd000c192170355eecb1c3d 100644 --- a/config.cmake +++ b/config.cmake @@ -6,10 +6,10 @@ IF (NOT Simox_CONFIGURED) # Set up build type IF(NOT CMAKE_BUILD_TYPE) - SET(CMAKE_BUILD_TYPE Debug CACHE STRING + SET(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." FORCE) - ENDIF(NOT CMAKE_BUILD_TYPE) + ENDIF() GET_FILENAME_COMPONENT (CurrentSimoxPath ${CMAKE_CURRENT_LIST_FILE} PATH) MESSAGE (STATUS "** Simox_DIR: ${CurrentSimoxPath}") @@ -50,7 +50,7 @@ IF (NOT Simox_CONFIGURED) ############################# VERSION ################################# set(Simox_MAJOR_VERSION 2) set(Simox_MINOR_VERSION 3) - set(Simox_PATCH_VERSION 72) + set(Simox_PATCH_VERSION 73) set(Simox_VERSION ${Simox_MAJOR_VERSION}.${Simox_MINOR_VERSION}.${Simox_PATCH_VERSION})