hyblocker--OpenVR-SpaceCalibrator/OpenVR-SpaceCalibrator/Configuration.cpp

#include "stdafx.h"
#include "Configuration.h"

#include <picojson.h>

#include <string>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>

static void UpgradeProfileV1(CalibrationContext &ctx);
static void ParseProfileV2(CalibrationContext &ctx, std::istream &stream);

static std::string ConfigFileName()
{
	char cruntimePath[MAX_PATH] = { 0 };
	unsigned int pathLen;
	vr::VR_GetRuntimePath(cruntimePath, MAX_PATH, &pathLen);
	std::string vrRuntimeConfigName(cruntimePath);
	return vrRuntimeConfigName + "\\..\\..\\..\\config\\01spacecalibrator\\calibration.json";
}

picojson::array FloatArray(const float *buf, int numFloats)
{
	picojson::array arr;

	for (int i = 0; i < numFloats; i++)
		arr.push_back(picojson::value(double(buf[i])));

	return arr;
}

void LoadFloatArray(const picojson::value &obj, float *buf, int numFloats)
{
	if (!obj.is<picojson::array>())
		throw std::runtime_error("expected array, got " + obj.to_str());

	auto &arr = obj.get<picojson::array>();
	if (arr.size() != numFloats)
		throw std::runtime_error("wrong buffer size");

	for (int i = 0; i < numFloats; i++)
		buf[i] = (float) arr[i].get<double>();
}

void LoadProfile(CalibrationContext &ctx)
{
	ctx.validProfile = false;

	std::ifstream file(ConfigFileName());
	if (!file.good())
	{
		std::cout << "Profile missing, trying fallback path" << std::endl;
		// Fallback to working directory, which was the default for a long time.
		file = std::ifstream("openvr_space_calibration.json");
	}


	if (!file.good())
	{
		std::cout << "Fallback profile missing, trying V1 path" << std::endl;
		UpgradeProfileV1(ctx);
		return;
	}

	try
	{
		ParseProfileV2(ctx, file);
		std::cout << "Loaded profile" << std::endl;
	}
	catch (const std::runtime_error &e)
	{
		std::cerr << "Error loading profile: " << e.what() << std::endl;
	}
}

static void ParseProfileV2(CalibrationContext &ctx, std::istream &stream)
{
	picojson::value v;
	std::string err = picojson::parse(v, stream);
	if (!err.empty())
		throw std::runtime_error(err);

	auto arr = v.get<picojson::array>();
	if (arr.size() < 1)
		throw std::runtime_error("no profiles in file");

	auto obj = arr[0].get<picojson::object>();

	ctx.referenceTrackingSystem = obj["reference_tracking_system"].get<std::string>();
	ctx.targetTrackingSystem = obj["target_tracking_system"].get<std::string>();
	ctx.calibratedRotation(0) = obj["roll"].get<double>();
	ctx.calibratedRotation(1) = obj["yaw"].get<double>();
	ctx.calibratedRotation(2) = obj["pitch"].get<double>();
	ctx.calibratedTranslation(0) = obj["x"].get<double>();
	ctx.calibratedTranslation(1) = obj["y"].get<double>();
	ctx.calibratedTranslation(2) = obj["z"].get<double>();

	if (obj["chaperone"].is<picojson::object>())
	{
		auto chaperone = obj["chaperone"].get<picojson::object>();
		ctx.chaperone.autoApply = chaperone["auto_apply"].get<bool>();

		LoadFloatArray(chaperone["play_space_size"], ctx.chaperone.playSpaceSize.v, 2);

		LoadFloatArray(
			chaperone["standing_center"],
			(float *) ctx.chaperone.standingCenter.m,
			sizeof(ctx.chaperone.standingCenter.m) / sizeof(float)
		);

		if (!chaperone["geometry"].is<picojson::array>())
			throw std::runtime_error("chaperone geometry is not an array");

		auto &geometry = chaperone["geometry"].get<picojson::array>();

		if (geometry.size() > 0)
		{
			ctx.chaperone.geometry.resize(geometry.size() * sizeof(float) / sizeof(ctx.chaperone.geometry[0]));
			LoadFloatArray(chaperone["geometry"], (float *) ctx.chaperone.geometry.data(), geometry.size());

			ctx.chaperone.valid = true;
		}
	}

	ctx.validProfile = true;
}

void SaveProfile(CalibrationContext &ctx)
{
	std::cout << "Saving profile to " << ConfigFileName() << std::endl;

	std::ofstream file(ConfigFileName());

	if (!ctx.validProfile)
	{
		file << "[]";
		return;
	}

	picojson::object profile;
	profile["reference_tracking_system"].set<std::string>(ctx.referenceTrackingSystem);
	profile["target_tracking_system"].set<std::string>(ctx.targetTrackingSystem);
	profile["roll"].set<double>(ctx.calibratedRotation(0));
	profile["yaw"].set<double>(ctx.calibratedRotation(1));
	profile["pitch"].set<double>(ctx.calibratedRotation(2));
	profile["x"].set<double>(ctx.calibratedTranslation(0));
	profile["y"].set<double>(ctx.calibratedTranslation(1));
	profile["z"].set<double>(ctx.calibratedTranslation(2));

	if (ctx.chaperone.valid)
	{
		picojson::object chaperone;
		chaperone["auto_apply"].set<bool>(ctx.chaperone.autoApply);
		chaperone["play_space_size"].set<picojson::array>(FloatArray(ctx.chaperone.playSpaceSize.v, 2));

		chaperone["standing_center"].set<picojson::array>(FloatArray(
			(float *) ctx.chaperone.standingCenter.m,
			sizeof(ctx.chaperone.standingCenter.m) / sizeof(float)
		));

		chaperone["geometry"].set<picojson::array>(FloatArray(
			(float *) ctx.chaperone.geometry.data(),
			sizeof(ctx.chaperone.geometry[0]) / sizeof(float) * ctx.chaperone.geometry.size()
		));

		profile["chaperone"].set<picojson::object>(chaperone);
	}

	picojson::value profileV;
	profileV.set<picojson::object>(profile);

	picojson::array profiles;
	profiles.push_back(profileV);

	picojson::value profilesV;
	profilesV.set<picojson::array>(profiles);

	file << profilesV.serialize(true);
}

static void UpgradeProfileV1(CalibrationContext &ctx)
{
	std::ifstream file("openvr_space_calibration.txt");
	ctx.validProfile = false;

	if (!file.good())
		return;

	file
		>> ctx.targetTrackingSystem
		>> ctx.calibratedRotation(1) // yaw
		>> ctx.calibratedRotation(2) // pitch
		>> ctx.calibratedRotation(0) // roll
		>> ctx.calibratedTranslation(0) // x
		>> ctx.calibratedTranslation(1) // y
		>> ctx.calibratedTranslation(2); // z

	ctx.validProfile = true;

	SaveProfile(ctx);

	file.close();
	std::remove("openvr_space_calibration.txt");
}