298 lines
10 KiB
C++
298 lines
10 KiB
C++
#include "ClientState.h"
|
||
|
||
|
||
void ClientState::simulate_physics(size_t delta) {
|
||
if (discreteMag > 0.01f)
|
||
{
|
||
float rad = static_cast<float>(discreteAngle) * static_cast<float>(M_PI) / 180.0f;
|
||
|
||
// Целевая угловая скорость (дискретная сила определяет модуль вектора)
|
||
// Вектор {cos, sin, 0} дает нам направление отклонения джойстика
|
||
Eigen::Vector3f targetAngularVelDir(sinf(rad), cosf(rad), 0.0f);
|
||
Eigen::Vector3f targetAngularVelocity = targetAngularVelDir * discreteMag;
|
||
|
||
Eigen::Vector3f diffVel = targetAngularVelocity - currentAngularVelocity;
|
||
float diffLen = diffVel.norm();
|
||
|
||
if (diffLen > 0.0001f) {
|
||
// Вычисляем, на сколько мы можем изменить скорость в этом кадре
|
||
float maxChange = ANGULAR_ACCEL * static_cast<float>(delta);
|
||
|
||
if (diffLen <= maxChange) {
|
||
// Если до цели осталось меньше, чем шаг ускорения — просто прыгаем в цель
|
||
currentAngularVelocity = targetAngularVelocity;
|
||
}
|
||
|
||
else {
|
||
// Линейно двигаемся в сторону целевого вектора
|
||
currentAngularVelocity += (diffVel / diffLen) * maxChange;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
float currentSpeed = currentAngularVelocity.norm();
|
||
|
||
if (currentSpeed > 0.0001f) {
|
||
float drop = ANGULAR_ACCEL * static_cast<float>(delta);
|
||
if (currentSpeed <= drop) {
|
||
currentAngularVelocity = Eigen::Vector3f::Zero();
|
||
}
|
||
else {
|
||
// Уменьшаем модуль вектора, сохраняя направление
|
||
currentAngularVelocity -= (currentAngularVelocity / currentSpeed) * drop;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
float distToCenter = position.norm(); // Расстояние до {0,0,0}
|
||
float landingZone = PLANET_RADIUS * PLANET_ALIGN_ZONE;
|
||
|
||
if (distToCenter <= landingZone) {
|
||
Eigen::Vector3f planetNormal = position.normalized();
|
||
|
||
// --- 1. ВЫРАВНИВАНИЕ КРЕНА (Roll - ось Z) ---
|
||
Eigen::Vector3f localX = rotation.col(0);
|
||
float rollError = localX.dot(planetNormal);
|
||
|
||
if (std::abs(rollError) > 0.001f) {
|
||
currentAngularVelocity.z() -= rollError * PLANET_ANGULAR_ACCEL * delta;
|
||
currentAngularVelocity.z() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
|
||
std::min(currentAngularVelocity.z(), PLANET_MAX_ANGULAR_VELOCITY));
|
||
}
|
||
|
||
// --- 2. ОГРАНИЧЕНИЕ ТАНГАЖА (Pitch - ось X) ---
|
||
// Нос корабля в локальных координатах — это -Z (третий столбец матрицы)
|
||
Eigen::Vector3f forwardDir = -rotation.col(2);
|
||
|
||
// В твоем случае dot < 0 означает, что нос направлен К планете
|
||
float pitchSin = forwardDir.dot(planetNormal);
|
||
float currentPitchAngle = asinf(std::clamp(pitchSin, -1.0f, 1.0f));
|
||
|
||
// Лимит у нас M_PI / 6.0 (примерно 0.523)
|
||
// По твоим данным: -0.89 < -0.523, значит мы превысили наклон вниз
|
||
if (currentPitchAngle < -PITCH_LIMIT) {
|
||
// Вычисляем ошибку (насколько мы ушли "ниже" лимита)
|
||
// -0.89 - (-0.52) = -0.37
|
||
float pitchError = currentPitchAngle + PITCH_LIMIT;
|
||
|
||
// Теперь важно: нам нужно ПОДНЯТЬ нос.
|
||
// Если pitchError отрицательный, а нам нужно уменьшить вращение,
|
||
// пробуем прибавлять или вычитать в зависимости от твоей оси X.
|
||
// Судя по стандартной логике Eigen, нам нужно ПЛЮСОВАТЬ:
|
||
currentAngularVelocity.x() -= pitchError * PLANET_ANGULAR_ACCEL * delta;
|
||
}
|
||
}
|
||
else {
|
||
// Вне зоны: тормозим Z (крен) И X (тангаж), если они были активны
|
||
float drop = ANGULAR_ACCEL * delta;
|
||
|
||
if (std::abs(currentAngularVelocity[2]) > 0.0001f) {
|
||
if (std::abs(currentAngularVelocity[2]) <= drop) {
|
||
currentAngularVelocity[2] = 0.0f;
|
||
}
|
||
else {
|
||
currentAngularVelocity[2] -= (currentAngularVelocity[2] > 0 ? 1.0f : -1.0f) * drop;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Ограничение скорости
|
||
currentAngularVelocity.x() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
|
||
std::min(currentAngularVelocity.x(), PLANET_MAX_ANGULAR_VELOCITY));
|
||
// Ограничение скорости
|
||
currentAngularVelocity.y() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
|
||
std::min(currentAngularVelocity.y(), PLANET_MAX_ANGULAR_VELOCITY));
|
||
// Ограничение скорости
|
||
currentAngularVelocity.z() = std::max(-PLANET_MAX_ANGULAR_VELOCITY,
|
||
std::min(currentAngularVelocity.z(), PLANET_MAX_ANGULAR_VELOCITY));
|
||
|
||
float speedScale = currentAngularVelocity.norm();
|
||
if (speedScale > 0.0001f) {
|
||
// Коэффициент чувствительности вращения
|
||
|
||
float deltaAlpha = speedScale * static_cast<float>(delta) * ROTATION_SENSITIVITY;
|
||
|
||
Eigen::Vector3f axis = currentAngularVelocity.normalized();
|
||
Eigen::Quaternionf rotateQuat(Eigen::AngleAxisf(deltaAlpha, axis));
|
||
|
||
rotation = rotation * rotateQuat.toRotationMatrix();
|
||
}
|
||
|
||
|
||
// 4. Линейное изменение линейной скорости
|
||
float shipDesiredVelocity = selectedVelocity * 100.f;
|
||
|
||
if (velocity < shipDesiredVelocity)
|
||
{
|
||
velocity += delta * SHIP_ACCEL;
|
||
if (velocity > shipDesiredVelocity)
|
||
{
|
||
velocity = shipDesiredVelocity;
|
||
}
|
||
}
|
||
else if (velocity > shipDesiredVelocity)
|
||
{
|
||
velocity -= delta * SHIP_ACCEL;
|
||
if (velocity < shipDesiredVelocity)
|
||
{
|
||
velocity = shipDesiredVelocity;
|
||
}
|
||
}
|
||
|
||
if (fabs(velocity) > 0.01f)
|
||
{
|
||
Eigen::Vector3f velocityDirection = { 0,0, -velocity * delta / 1000.f };
|
||
Eigen::Vector3f velocityDirectionAdjusted = rotation * velocityDirection;
|
||
position = position + velocityDirectionAdjusted;
|
||
}
|
||
}
|
||
|
||
void ClientState::apply_lag_compensation(std::chrono::system_clock::time_point nowTime) {
|
||
|
||
// 2. Вычисляем задержку
|
||
long long deltaMs = 0;
|
||
if (nowTime > lastUpdateServerTime) {
|
||
deltaMs = std::chrono::duration_cast<std::chrono::milliseconds>(nowTime - lastUpdateServerTime).count();
|
||
}
|
||
|
||
long long deltaMsLeftover = deltaMs;
|
||
|
||
while (deltaMsLeftover > 0)
|
||
{
|
||
long long miniDelta = 50;
|
||
simulate_physics(miniDelta);
|
||
deltaMsLeftover -= miniDelta;
|
||
}
|
||
|
||
/*
|
||
// 3. Защита от слишком больших скачков (Clamp)
|
||
// Если лаг более 500мс, ограничиваем его, чтобы избежать резких рывков
|
||
long long final_lag_ms = deltaMs;//min(deltaMs, 500ll);
|
||
|
||
if (final_lag_ms > 0) {
|
||
// Доматываем симуляцию на величину задержки
|
||
// Мы предполагаем, что за это время параметры управления не менялись
|
||
simulate_physics(final_lag_ms);
|
||
}*/
|
||
}
|
||
|
||
void ClientState::handle_full_sync(const std::vector<std::string>& parts, int startFrom) {
|
||
// Позиция
|
||
position = { std::stof(parts[startFrom]), std::stof(parts[startFrom + 1]), std::stof(parts[startFrom + 2]) };
|
||
|
||
Eigen::Quaternionf q(
|
||
std::stof(parts[startFrom + 3]),
|
||
std::stof(parts[startFrom + 4]),
|
||
std::stof(parts[startFrom + 5]),
|
||
std::stof(parts[startFrom + 6]));
|
||
rotation = q.toRotationMatrix();
|
||
|
||
currentAngularVelocity = Eigen::Vector3f{
|
||
std::stof(parts[startFrom + 7]),
|
||
std::stof(parts[startFrom + 8]),
|
||
std::stof(parts[startFrom + 9]) };
|
||
velocity = std::stof(parts[startFrom + 10]);
|
||
selectedVelocity = std::stoi(parts[startFrom + 11]);
|
||
discreteMag = std::stof(parts[startFrom + 12]);
|
||
discreteAngle = std::stoi(parts[startFrom + 13]);
|
||
}
|
||
|
||
std::string ClientState::formPingMessageContent()
|
||
{
|
||
Eigen::Quaternionf q(rotation);
|
||
|
||
std::string pingMsg = std::to_string(position.x()) + ":"
|
||
+ std::to_string(position.y()) + ":"
|
||
+ std::to_string(position.z()) + ":"
|
||
+ std::to_string(q.w()) + ":"
|
||
+ std::to_string(q.x()) + ":"
|
||
+ std::to_string(q.y()) + ":"
|
||
+ std::to_string(q.z()) + ":"
|
||
+ std::to_string(currentAngularVelocity.x()) + ":"
|
||
+ std::to_string(currentAngularVelocity.y()) + ":"
|
||
+ std::to_string(currentAngularVelocity.z()) + ":"
|
||
+ std::to_string(velocity) + ":"
|
||
+ std::to_string(selectedVelocity) + ":"
|
||
+ std::to_string(discreteMag) + ":" // Используем те же static переменные из блока ROT
|
||
+ std::to_string(discreteAngle);
|
||
|
||
return pingMsg;
|
||
}
|
||
|
||
|
||
void ClientStateInterval::add_state(const ClientState& state)
|
||
{
|
||
auto nowTime = std::chrono::system_clock::now();
|
||
|
||
if (timedStates.size() > 0 && timedStates[timedStates.size() - 1].lastUpdateServerTime == state.lastUpdateServerTime)
|
||
{
|
||
timedStates[timedStates.size() - 1] = state;
|
||
}
|
||
else
|
||
{
|
||
timedStates.push_back(state);
|
||
}
|
||
|
||
auto cutoff_time = nowTime - std::chrono::milliseconds(CUTOFF_TIME);
|
||
|
||
while (timedStates.size() > 0 && timedStates[0].lastUpdateServerTime < cutoff_time)
|
||
{
|
||
timedStates.erase(timedStates.begin());
|
||
}
|
||
}
|
||
|
||
bool ClientStateInterval::canFetchClientStateAtTime(std::chrono::system_clock::time_point targetTime) const
|
||
{
|
||
if (timedStates.empty())
|
||
{
|
||
return false;
|
||
}
|
||
if (timedStates[0].lastUpdateServerTime > targetTime)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
ClientState ClientStateInterval::fetchClientStateAtTime(std::chrono::system_clock::time_point targetTime) const {
|
||
|
||
ClientState closestState;
|
||
|
||
if (timedStates.empty())
|
||
{
|
||
throw std::runtime_error("No timed client states available");
|
||
return closestState;
|
||
}
|
||
if (timedStates[0].lastUpdateServerTime > targetTime)
|
||
{
|
||
throw std::runtime_error("Found time but it is in future");
|
||
return closestState;
|
||
}
|
||
if (timedStates.size() == 1)
|
||
{
|
||
closestState = timedStates[0];
|
||
closestState.apply_lag_compensation(targetTime);
|
||
return closestState;
|
||
}
|
||
|
||
|
||
for (size_t i = 0; i < timedStates.size() - 1; ++i)
|
||
{
|
||
const auto& earlierState = timedStates[i];
|
||
const auto& laterState = timedStates[i + 1];
|
||
if (earlierState.lastUpdateServerTime <= targetTime && laterState.lastUpdateServerTime >= targetTime)
|
||
{
|
||
closestState = earlierState;
|
||
closestState.apply_lag_compensation(targetTime);
|
||
return closestState;
|
||
}
|
||
}
|
||
|
||
closestState = timedStates[timedStates.size() - 1];
|
||
closestState.apply_lag_compensation(targetTime);
|
||
return closestState;
|
||
}
|