n_dead_reckoning.h

Go to the documentation of this file.

/*
 * Nilorea Library
 * Copyright (C) 2005-2026 Castagnier Mickael
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied. See the License for the specific language governing
 * permissions and limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
 
#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 */