Files
WiVRn/server/driver/polynomial_interpolator.h

218 lines
5.8 KiB
C++

/*
* WiVRn VR streaming
* Copyright (C) 2025 Guillaume Meunier <[email protected]>
* Copyright (C) 2025 Patrick Nicolas <[email protected]>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
// xlib sucks
#ifdef Success
#undef Success
#endif
#ifdef Complex
#undef Complex
#endif
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/QR>
#include <algorithm>
#include <limits>
#include <optional>
#include <ranges>
#include "openxr/openxr.h"
#include "wivrn_config.h"
template <int N, bool quaternion = false, int polynomial_order = 2, int stored_samples = 30>
class polynomial_interpolator
{
public:
using value_type = Eigen::Vector<float, N>;
struct sample
{
XrTime production_timestamp = std::numeric_limits<XrTime>::lowest();
XrTime timestamp = std::numeric_limits<XrTime>::lowest();
std::optional<value_type> y;
std::optional<value_type> dy;
};
XrDuration window = 30'000'000; // time span of samples to use for extrapolation
float time_constant = 0.001; // duration (s) to convert velocities into positions
private:
std::array<sample, stored_samples> data;
// q and -q are the same rotation, use the sign of the sample closest in time
void align_sign(sample & stored)
{
if constexpr (quaternion)
{
if (stored.y)
{
sample * closest = nullptr;
for (auto & i: data)
{
if (&i == &stored or not i.y)
continue;
if (not closest)
closest = &i;
else if (std::abs(closest->timestamp - stored.timestamp) > std::abs(i.timestamp - stored.timestamp))
closest = &i;
}
if (closest and closest->y->dot(*stored.y) < 0)
*stored.y *= -1;
}
}
}
public:
polynomial_interpolator() = default;
polynomial_interpolator(XrDuration window, float time_constant) : window(window), time_constant(time_constant) {}
void reset()
{
data.fill({});
}
void add_sample(const sample & s)
{
sample * oldest = nullptr;
for (auto & i: data)
{
// Avoid samples too close to each other
if (std::abs(i.timestamp - s.timestamp) < 2'000'000)
{
if (i.production_timestamp < s.production_timestamp)
{
i = s;
align_sign(i);
}
return;
}
if ((not oldest) or oldest->production_timestamp > i.production_timestamp)
oldest = &i;
}
*oldest = s;
align_sign(*oldest);
}
std::pair<XrTime, XrTime> get_bounds() const
{
std::pair<XrTime, XrTime> result{
std::numeric_limits<XrTime>::max(),
std::numeric_limits<XrTime>::lowest(),
};
for (const auto & sample: data)
{
if (sample.y)
{
result.first = std::min(result.first, sample.timestamp);
result.second = std::max(result.second, sample.timestamp);
}
}
return result;
}
sample get_at(XrTime timestamp) const
{
Eigen::Matrix<float, 2 * stored_samples, polynomial_order + 1> A;
Eigen::Matrix<float, 2 * stored_samples, N> b;
const XrTime production_timestamp = std::ranges::max(data | std::ranges::views::transform(&sample::production_timestamp));
// Maximum is the minimum of now + max_extrapolation_ns (outside of this function)
// and production_ts + 1.1 * max_extrapolation_ns
// This allows a small buffer so that polynomial extrapolation fills the gap of networking hiccups
timestamp = std::min(timestamp, production_timestamp + (wivrn::max_extrapolation_ns * 11) / 10);
const sample * closest = data.data();
int row = 0;
for (const auto && [i, sample]: std::ranges::enumerate_view(data))
{
if (std::abs(closest->timestamp - timestamp) > std::abs(sample.timestamp - timestamp))
closest = &sample;
if (not sample.y)
continue;
int abs_Δt = std::abs(sample.timestamp - timestamp);
float weight = 1. / (1. + std::pow(abs_Δt / float(window), 3.));
float Δt = (sample.timestamp - timestamp) * 1.e-9;
float Δtⁱ = 1;
for (int i = 0; i <= polynomial_order; ++i)
{
A(row, i) = weight * Δtⁱ;
Δtⁱ *= Δt;
}
b.template block<1, N>(row, 0) = *sample.y * weight;
row++;
if (sample.dy.has_value())
{
A(row, 0) = 0;
Δtⁱ = 1;
for (int i = 1; i <= polynomial_order; ++i)
{
A(row, i) = weight * time_constant * i * Δtⁱ;
Δtⁱ *= Δt;
}
b.template block<1, N>(row, 0) = *sample.dy * weight * time_constant;
row++;
}
}
// Data turned invalid, return it
if (not closest->y)
return *closest;
// Avoid returning obviously wrong data
if (std::abs(closest->timestamp - timestamp) > 1'000'000'000)
return {};
// Not enough data to extrapolate
if (row < 2)
{
if (closest->y and std::abs(closest->production_timestamp - timestamp) < 1'000'000'000)
return *closest;
return {};
}
auto Aprime = A.block(0, 0, row, polynomial_order + 1);
auto bprime = b.block(0, 0, row, N);
Eigen::Matrix<float, N, polynomial_order + 1> sol;
if (row <= polynomial_order)
// Underdetermined case
sol = Eigen::ColPivHouseholderQR<decltype(Aprime)>{Aprime}.solve(bprime).transpose();
else
sol = (Aprime.transpose() * Aprime).ldlt().solve(Aprime.transpose() * bprime).transpose();
return sample{
.production_timestamp = production_timestamp,
.timestamp = timestamp,
.y = sol.template block<N, 1>(0, 0),
.dy = sol.template block<N, 1>(0, 1),
};
}
};