n_dead_reckoning.h

Go to the documentation of this file.

/*
 * Nilorea Library
 * Copyright (C) 2005-2026 Castagnier Mickael
 *
 * 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/>.
 */
 
#ifndef N_DEAD_RECKONING_H
#define N_DEAD_RECKONING_H
 
#ifdef __cplusplus
extern "C" {
#endif
 
#include <stdbool.h>
#include <math.h>
 
/*---------------------------------------------------------------------------
 * 3D Vector type for dead reckoning
 *-------------------------------------------------------------------------*/
 
typedef struct DR_VEC3 {
    double x; 
    double y; 
    double z; 
} DR_VEC3;
 
/*---------------------------------------------------------------------------
 * Enumerations
 *-------------------------------------------------------------------------*/
 
typedef enum DR_ALGO {
    DR_ALGO_STATIC = 0, 
    DR_ALGO_VEL = 1,    
    DR_ALGO_VEL_ACC = 2 
} DR_ALGO;
 
typedef enum DR_BLEND {
    DR_BLEND_SNAP = 0, 
    DR_BLEND_PVB = 1,  
    DR_BLEND_CUBIC = 2 
} DR_BLEND;
 
/*---------------------------------------------------------------------------
 * Kinematic state snapshot
 *-------------------------------------------------------------------------*/
 
typedef struct DR_STATE {
    DR_VEC3 pos; 
    DR_VEC3 vel; 
    DR_VEC3 acc; 
    double time; 
} DR_STATE;
 
/*---------------------------------------------------------------------------
 * Dead reckoning entity
 *-------------------------------------------------------------------------*/
 
typedef struct DR_ENTITY {
    /* --- Configuration --- */
    DR_ALGO algo;         
    DR_BLEND blend_mode;  
    double pos_threshold; 
    double blend_time;    
    /* --- Last known authoritative state (received from network) --- */
    DR_STATE last_known; 
    /* --- State at the moment we received the last update --- */
    DR_STATE prev_predicted; 
    /* --- Convergence tracking --- */
    double blend_start; 
    bool blending;      
    /* --- Cubic Bezier control points (computed on update) --- */
    DR_VEC3 bezier_p0; 
    DR_VEC3 bezier_p1; 
    DR_VEC3 bezier_p2; 
    DR_VEC3 bezier_p3; 
    /* --- Output --- */
    DR_VEC3 display_pos; 
    DR_VEC3 display_vel; 
    /* --- Statistics --- */
    int update_count; 
} DR_ENTITY;
 
/*---------------------------------------------------------------------------
 * Vector operations (inline helpers)
 *-------------------------------------------------------------------------*/
 
static inline DR_VEC3 dr_vec3(double x, double y, double z) {
    DR_VEC3 v = {x, y, z};
    return v;
}
 
static inline DR_VEC3 dr_vec3_zero(void) {
    DR_VEC3 v = {0.0, 0.0, 0.0};
    return v;
}
 
static inline DR_VEC3 dr_vec3_add(DR_VEC3 a, DR_VEC3 b) {
    DR_VEC3 v = {a.x + b.x, a.y + b.y, a.z + b.z};
    return v;
}
 
static inline DR_VEC3 dr_vec3_sub(DR_VEC3 a, DR_VEC3 b) {
    DR_VEC3 v = {a.x - b.x, a.y - b.y, a.z - b.z};
    return v;
}
 
static inline DR_VEC3 dr_vec3_scale(DR_VEC3 v, double s) {
    DR_VEC3 r = {v.x * s, v.y * s, v.z * s};
    return r;
}
 
static inline DR_VEC3 dr_vec3_lerp(DR_VEC3 a, DR_VEC3 b, double t) {
    DR_VEC3 r = {
        a.x + (b.x - a.x) * t,
        a.y + (b.y - a.y) * t,
        a.z + (b.z - a.z) * t};
    return r;
}
 
static inline double dr_vec3_length_sq(DR_VEC3 v) {
    return v.x * v.x + v.y * v.y + v.z * v.z;
}
 
static inline double dr_vec3_length(DR_VEC3 v) {
    return sqrt(dr_vec3_length_sq(v));
}
 
static inline double dr_vec3_distance(DR_VEC3 a, DR_VEC3 b) {
    return dr_vec3_length(dr_vec3_sub(a, b));
}
 
/*---------------------------------------------------------------------------
 * API Functions
 *-------------------------------------------------------------------------*/
 
DR_ENTITY* dr_entity_create(DR_ALGO algo, DR_BLEND blend_mode, double pos_threshold, double blend_time);
void dr_entity_destroy(DR_ENTITY** entity_ptr);
void dr_entity_receive_state(DR_ENTITY* entity,
                             const DR_VEC3* pos,
                             const DR_VEC3* vel,
                             const DR_VEC3* acc,
                             double time);
void dr_entity_compute(DR_ENTITY* entity, double time, DR_VEC3* out_pos);
DR_VEC3 dr_entity_extrapolate(const DR_ENTITY* entity, const DR_STATE* state, double dt);
bool dr_entity_check_threshold(const DR_ENTITY* entity,
                               const DR_VEC3* true_pos,
                               const DR_VEC3* true_vel,
                               const DR_VEC3* true_acc,
                               double time);
void dr_entity_set_algo(DR_ENTITY* entity, DR_ALGO algo);
void dr_entity_set_blend_mode(DR_ENTITY* entity, DR_BLEND blend_mode);
void dr_entity_set_threshold(DR_ENTITY* entity, double threshold);
void dr_entity_set_blend_time(DR_ENTITY* entity, double blend_time);
void dr_entity_set_position(DR_ENTITY* entity,
                            const DR_VEC3* pos,
                            const DR_VEC3* vel,
                            const DR_VEC3* acc,
                            double time);
 
#ifdef __cplusplus
}
#endif
 
#endif /* N_DEAD_RECKONING_H */