src/n_trajectory.c

Trajectory interpolation and dead reckoning for 2D/3D. More...

Functions

	Name
void	trajectory_load_segment(TRAJECTORY * traj, double time_val) Load the correct spline segment for a given time when using multi-point paths.
void	trajectory_compute_tangents(TRAJECTORY * traj) Recompute the Hermite tangent vectors from current start/end states.
double	hermite_h11(double s) Hermite basis function h11: weights the end tangent.
double	hermite_h10(double s) Hermite basis function h10: weights the start tangent.
double	hermite_h01(double s) Hermite basis function h01: weights the end position.
double	hermite_h00(double s) Hermite basis function h00: weights the start position.
double	hermite_dh11(double s) First derivative of h11: dh11/ds = 3s^2 - 2s.
double	hermite_dh10(double s) First derivative of h10: dh10/ds = 3s^2 - 4s + 1.
double	hermite_dh01(double s) First derivative of h01: dh01/ds = -6s^2 + 6s.
double	hermite_dh00(double s) First derivative of h00: dh00/ds = 6s^2 - 6s.
double	hermite_d2h11(double s) Second derivative of h11: d2h11/ds2 = 6s - 2.
double	hermite_d2h10(double s) Second derivative of h10: d2h10/ds2 = 6s - 4.
double	hermite_d2h01(double s) Second derivative of h01: d2h01/ds2 = -12s + 6.
double	hermite_d2h00(double s) Second derivative of h00: d2h00/ds2 = 12s - 6.

Detailed Description

Trajectory interpolation and dead reckoning for 2D/3D.

Author: Castagnier Mickael

Version: 1.0

Date: 28/02/2026

Implements the "Defeating Lag With Cubic Splines" approach from GameDev.net Article 914 by Nicholas Van Caldwell, using cubic Hermite spline interpolation with quadratic extrapolation.

Cubic Hermite spline formula: P(s) = h00(s)*P0 + h10(s)*M0 + h01(s)*P1 + h11(s)*M1

Where s is normalized time in [0,1], P0/P1 are endpoint positions, M0/M1 are tangent vectors (velocity * duration), and: h00(s) = 2s^3 - 3s^2 + 1 (start position weight) h10(s) = s^3 - 2s^2 + s (start tangent weight) h01(s) = -2s^3 + 3s^2 (end position weight) h11(s) = s^3 - s^2 (end tangent weight)

Functions Documentation

function trajectory_load_segment

static void trajectory_load_segment(
    TRAJECTORY * traj,
    double time_val
)

Load the correct spline segment for a given time when using multi-point paths.

Parameters:

• traj The trajectory (must have nb_points >= 2)
• time_val The time to find the segment for

Searches forward/backward from current_segment for efficiency with sequential access. Updates traj->start, end, start_time, end_time, duration, and tangents for the found segment.

function trajectory_compute_tangents

static void trajectory_compute_tangents(
    TRAJECTORY * traj
)

Recompute the Hermite tangent vectors from current start/end states.

Parameters:

• traj The trajectory to update tangents for

Tangents are: m = velocity * duration (for position) and angular_speed * duration (for orientation)

function hermite_h11

static double hermite_h11(
    double s
)

Hermite basis function h11: weights the end tangent.

Parameters:

• s normalized time in [0,1]

Return: h11(s) = s^3 - s^2

function hermite_h10

static double hermite_h10(
    double s
)

Hermite basis function h10: weights the start tangent.

Parameters:

• s normalized time in [0,1]

Return: h10(s) = s^3 - 2s^2 + s

function hermite_h01

static double hermite_h01(
    double s
)

Hermite basis function h01: weights the end position.

Parameters:

• s normalized time in [0,1]

Return: h01(s) = -2s^3 + 3s^2

function hermite_h00

static double hermite_h00(
    double s
)

Hermite basis function h00: weights the start position.

Parameters:

• s normalized time in [0,1]

Return: h00(s) = 2s^3 - 3s^2 + 1

function hermite_dh11

static double hermite_dh11(
    double s
)

First derivative of h11: dh11/ds = 3s^2 - 2s.

Parameters:

• s normalized time in [0,1]

Return: dh11(s)

function hermite_dh10

static double hermite_dh10(
    double s
)

First derivative of h10: dh10/ds = 3s^2 - 4s + 1.

Parameters:

• s normalized time in [0,1]

Return: dh10(s)

function hermite_dh01

static double hermite_dh01(
    double s
)

First derivative of h01: dh01/ds = -6s^2 + 6s.

Parameters:

• s normalized time in [0,1]

Return: dh01(s)

function hermite_dh00

static double hermite_dh00(
    double s
)

First derivative of h00: dh00/ds = 6s^2 - 6s.

Parameters:

• s normalized time in [0,1]

Return: dh00(s)

function hermite_d2h11

static double hermite_d2h11(
    double s
)

Second derivative of h11: d2h11/ds2 = 6s - 2.

Parameters:

• s normalized time in [0,1]

Return: d2h11(s)

function hermite_d2h10

static double hermite_d2h10(
    double s
)

Second derivative of h10: d2h10/ds2 = 6s - 4.

Parameters:

• s normalized time in [0,1]

Return: d2h10(s)

function hermite_d2h01

static double hermite_d2h01(
    double s
)

Second derivative of h01: d2h01/ds2 = -12s + 6.

Parameters:

• s normalized time in [0,1]

Return: d2h01(s)

function hermite_d2h00

static double hermite_d2h00(
    double s
)

Second derivative of h00: d2h00/ds2 = 12s - 6.

Parameters:

• s normalized time in [0,1]

Return: d2h00(s)

Source code

/*
 * 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
 */


#include "nilorea/n_trajectory.h"
#include <math.h>

static double hermite_h00(double s) {
    return 2.0 * s * s * s - 3.0 * s * s + 1.0;
}

static double hermite_h10(double s) {
    return s * s * s - 2.0 * s * s + s;
}

static double hermite_h01(double s) {
    return -2.0 * s * s * s + 3.0 * s * s;
}

static double hermite_h11(double s) {
    return s * s * s - s * s;
}

static double hermite_dh00(double s) {
    return 6.0 * s * s - 6.0 * s;
}

static double hermite_dh10(double s) {
    return 3.0 * s * s - 4.0 * s + 1.0;
}

static double hermite_dh01(double s) {
    return -6.0 * s * s + 6.0 * s;
}

static double hermite_dh11(double s) {
    return 3.0 * s * s - 2.0 * s;
}

static double hermite_d2h00(double s) {
    return 12.0 * s - 6.0;
}

static double hermite_d2h10(double s) {
    return 6.0 * s - 4.0;
}

static double hermite_d2h01(double s) {
    return -12.0 * s + 6.0;
}

static double hermite_d2h11(double s) {
    return 6.0 * s - 2.0;
}

static void trajectory_compute_tangents(TRAJECTORY* traj) {
    for (int i = 0; i < traj->nb_components; i++) {
        traj->m0[i] = traj->start.speed[i] * traj->duration;
        traj->m1[i] = traj->end.speed[i] * traj->duration;
        traj->m0_orient[i] = traj->start.angular_speed[i] * traj->duration;
        traj->m1_orient[i] = traj->end.angular_speed[i] * traj->duration;
    }
}

static void trajectory_load_segment(TRAJECTORY* traj, double time_val) {
    if (traj->nb_points < 2) return;

    int seg = traj->current_segment;
    if (seg < 0) seg = 0;

    /* search forward from current segment */
    while (seg < traj->nb_points - 2 &&
           time_val >= traj->points[seg + 1].time_val) {
        seg++;
    }
    /* search backward if needed */
    while (seg > 0 && time_val < traj->points[seg].time_val) {
        seg--;
    }

    if (seg != traj->current_segment) {
        traj->current_segment = seg;
        memcpy(&traj->start, &traj->points[seg].state, sizeof(PHYSICS));
        memcpy(&traj->end, &traj->points[seg + 1].state, sizeof(PHYSICS));
        traj->start_time = traj->points[seg].time_val;
        traj->end_time = traj->points[seg + 1].time_val;
        traj->duration = traj->end_time - traj->start_time;
        trajectory_compute_tangents(traj);
    }
}

TRAJECTORY* trajectory_new(int nb_components) {
    if (nb_components != TRAJECTORY_2D && nb_components != TRAJECTORY_3D) {
        n_log(LOG_ERR, "nb_components must be TRAJECTORY_2D (2) or TRAJECTORY_3D (3), got %d", nb_components);
        return NULL;
    }

    TRAJECTORY* traj = NULL;
    Malloc(traj, TRAJECTORY, 1);
    __n_assert(traj, return NULL);

    traj->nb_components = nb_components;
    traj->mode = TRAJECTORY_BEFORE;
    traj->start_time = 0.0;
    traj->end_time = 0.0;
    traj->duration = 0.0;
    traj->current_time = 0.0;
    traj->points = NULL;
    traj->nb_points = 0;
    traj->nb_points_allocated = 0;
    traj->current_segment = -1;

    return traj;
} /* trajectory_new() */

void trajectory_delete(TRAJECTORY** traj) {
    __n_assert(traj, return);
    Free((*traj)->points);
    Free((*traj));
} /* trajectory_delete() */

int trajectory_set_start(TRAJECTORY* traj, const PHYSICS* state, double time_val) {
    __n_assert(traj, return FALSE);
    __n_assert(state, return FALSE);

    memcpy(&traj->start, state, sizeof(PHYSICS));
    traj->start_time = time_val;
    traj->duration = traj->end_time - traj->start_time;

    if (traj->duration > 0.0) {
        trajectory_compute_tangents(traj);
    }

    return TRUE;
} /* trajectory_set_start() */

int trajectory_set_end(TRAJECTORY* traj, const PHYSICS* state, double time_val) {
    __n_assert(traj, return FALSE);
    __n_assert(state, return FALSE);

    memcpy(&traj->end, state, sizeof(PHYSICS));
    traj->end_time = time_val;
    traj->duration = traj->end_time - traj->start_time;

    if (traj->duration <= 0.0) {
        n_log(LOG_ERR, "end_time (%g) must be greater than start_time (%g)", time_val, traj->start_time);
        return FALSE;
    }

    trajectory_compute_tangents(traj);

    return TRUE;
} /* trajectory_set_end() */

int trajectory_update(TRAJECTORY* traj, const PHYSICS* new_end, double time_val) {
    __n_assert(traj, return FALSE);
    __n_assert(new_end, return FALSE);

    if (time_val <= traj->end_time) {
        n_log(LOG_ERR, "new time (%g) must be greater than current end_time (%g)", time_val, traj->end_time);
        return FALSE;
    }

    /* shift: old end becomes new start */
    memcpy(&traj->start, &traj->end, sizeof(PHYSICS));
    traj->start_time = traj->end_time;

    /* set new end */
    memcpy(&traj->end, new_end, sizeof(PHYSICS));
    traj->end_time = time_val;
    traj->duration = traj->end_time - traj->start_time;

    trajectory_compute_tangents(traj);

    return TRUE;
} /* trajectory_update() */

int trajectory_compute(TRAJECTORY* traj, double time_val) {
    __n_assert(traj, return FALSE);

    trajectory_load_segment(traj, time_val);
    traj->current_time = time_val;
    int nc = traj->nb_components;

    if (traj->duration <= 0.0) {
        /* no valid interval: just copy start state */
        memcpy(&traj->current, &traj->start, sizeof(PHYSICS));
        traj->mode = TRAJECTORY_BEFORE;
        return TRUE;
    }

    if (time_val < traj->start_time) {
        /* before start: quadratic extrapolation backward from start state */
        traj->mode = TRAJECTORY_BEFORE;
        double dt = time_val - traj->start_time; /* negative */
        for (int i = 0; i < nc; i++) {
            traj->current.position[i] = traj->start.position[i] + traj->start.speed[i] * dt + 0.5 * traj->start.acceleration[i] * dt * dt;
            traj->current.speed[i] = traj->start.speed[i] + traj->start.acceleration[i] * dt;
            traj->current.acceleration[i] = traj->start.acceleration[i];
            traj->current.orientation[i] = traj->start.orientation[i] + traj->start.angular_speed[i] * dt + 0.5 * traj->start.angular_acceleration[i] * dt * dt;
            traj->current.angular_speed[i] = traj->start.angular_speed[i] + traj->start.angular_acceleration[i] * dt;
            traj->current.angular_acceleration[i] = traj->start.angular_acceleration[i];
        }
    } else if (time_val > traj->end_time) {
        /* after end: quadratic extrapolation forward from end state */
        traj->mode = TRAJECTORY_EXTRAP;
        double dt = time_val - traj->end_time;
        for (int i = 0; i < nc; i++) {
            traj->current.position[i] = traj->end.position[i] + traj->end.speed[i] * dt + 0.5 * traj->end.acceleration[i] * dt * dt;
            traj->current.speed[i] = traj->end.speed[i] + traj->end.acceleration[i] * dt;
            traj->current.acceleration[i] = traj->end.acceleration[i];
            traj->current.orientation[i] = traj->end.orientation[i] + traj->end.angular_speed[i] * dt + 0.5 * traj->end.angular_acceleration[i] * dt * dt;
            traj->current.angular_speed[i] = traj->end.angular_speed[i] + traj->end.angular_acceleration[i] * dt;
            traj->current.angular_acceleration[i] = traj->end.angular_acceleration[i];
        }
    } else {
        /* within interval: cubic Hermite spline interpolation */
        traj->mode = TRAJECTORY_INTERP;
        double s = (time_val - traj->start_time) / traj->duration;

        /* Hermite basis function values */
        double h00 = hermite_h00(s);
        double h10 = hermite_h10(s);
        double h01 = hermite_h01(s);
        double h11 = hermite_h11(s);

        /* first derivatives for velocity */
        double dh00 = hermite_dh00(s);
        double dh10 = hermite_dh10(s);
        double dh01 = hermite_dh01(s);
        double dh11 = hermite_dh11(s);

        /* second derivatives for acceleration */
        double d2h00 = hermite_d2h00(s);
        double d2h10 = hermite_d2h10(s);
        double d2h01 = hermite_d2h01(s);
        double d2h11 = hermite_d2h11(s);

        double inv_dur = 1.0 / traj->duration;
        double inv_dur2 = inv_dur * inv_dur;

        for (int i = 0; i < nc; i++) {
            /* position: P(s) = h00*P0 + h10*M0 + h01*P1 + h11*M1 */
            traj->current.position[i] = h00 * traj->start.position[i] + h10 * traj->m0[i] + h01 * traj->end.position[i] + h11 * traj->m1[i];

            /* velocity: V(t) = P'(s) / duration */
            traj->current.speed[i] = (dh00 * traj->start.position[i] + dh10 * traj->m0[i] + dh01 * traj->end.position[i] + dh11 * traj->m1[i]) * inv_dur;

            /* acceleration: A(t) = P''(s) / duration^2 */
            traj->current.acceleration[i] = (d2h00 * traj->start.position[i] + d2h10 * traj->m0[i] + d2h01 * traj->end.position[i] + d2h11 * traj->m1[i]) * inv_dur2;

            /* orientation: same Hermite approach */
            traj->current.orientation[i] = h00 * traj->start.orientation[i] + h10 * traj->m0_orient[i] + h01 * traj->end.orientation[i] + h11 * traj->m1_orient[i];

            /* angular speed: derivative of orientation spline / duration */
            traj->current.angular_speed[i] = (dh00 * traj->start.orientation[i] + dh10 * traj->m0_orient[i] + dh01 * traj->end.orientation[i] + dh11 * traj->m1_orient[i]) * inv_dur;

            /* angular acceleration: second derivative / duration^2 */
            traj->current.angular_acceleration[i] = (d2h00 * traj->start.orientation[i] + d2h10 * traj->m0_orient[i] + d2h01 * traj->end.orientation[i] + d2h11 * traj->m1_orient[i]) * inv_dur2;
        }
    }

    /* zero out unused components for safety */
    for (int i = nc; i < 3; i++) {
        traj->current.position[i] = 0.0;
        traj->current.speed[i] = 0.0;
        traj->current.acceleration[i] = 0.0;
        traj->current.orientation[i] = 0.0;
        traj->current.angular_speed[i] = 0.0;
        traj->current.angular_acceleration[i] = 0.0;
    }

    return TRUE;
} /* trajectory_compute() */

int trajectory_get_position(TRAJECTORY* traj, double time_val, VECTOR3D out) {
    __n_assert(traj, return FALSE);
    __n_assert(out, return FALSE);

    trajectory_load_segment(traj, time_val);
    int nc = traj->nb_components;

    if (traj->duration <= 0.0) {
        for (int i = 0; i < nc; i++) out[i] = traj->start.position[i];
        for (int i = nc; i < 3; i++) out[i] = 0.0;
        return TRUE;
    }

    if (time_val < traj->start_time) {
        double dt = time_val - traj->start_time;
        for (int i = 0; i < nc; i++) {
            out[i] = traj->start.position[i] + traj->start.speed[i] * dt + 0.5 * traj->start.acceleration[i] * dt * dt;
        }
    } else if (time_val > traj->end_time) {
        double dt = time_val - traj->end_time;
        for (int i = 0; i < nc; i++) {
            out[i] = traj->end.position[i] + traj->end.speed[i] * dt + 0.5 * traj->end.acceleration[i] * dt * dt;
        }
    } else {
        double s = (time_val - traj->start_time) / traj->duration;
        double h00 = hermite_h00(s);
        double h10 = hermite_h10(s);
        double h01 = hermite_h01(s);
        double h11 = hermite_h11(s);
        for (int i = 0; i < nc; i++) {
            out[i] = h00 * traj->start.position[i] + h10 * traj->m0[i] + h01 * traj->end.position[i] + h11 * traj->m1[i];
        }
    }

    for (int i = nc; i < 3; i++) out[i] = 0.0;
    return TRUE;
} /* trajectory_get_position() */

int trajectory_get_speed(TRAJECTORY* traj, double time_val, VECTOR3D out) {
    __n_assert(traj, return FALSE);
    __n_assert(out, return FALSE);

    trajectory_load_segment(traj, time_val);
    int nc = traj->nb_components;

    if (traj->duration <= 0.0) {
        for (int i = 0; i < nc; i++) out[i] = traj->start.speed[i];
        for (int i = nc; i < 3; i++) out[i] = 0.0;
        return TRUE;
    }

    if (time_val < traj->start_time) {
        double dt = time_val - traj->start_time;
        for (int i = 0; i < nc; i++) {
            out[i] = traj->start.speed[i] + traj->start.acceleration[i] * dt;
        }
    } else if (time_val > traj->end_time) {
        double dt = time_val - traj->end_time;
        for (int i = 0; i < nc; i++) {
            out[i] = traj->end.speed[i] + traj->end.acceleration[i] * dt;
        }
    } else {
        double s = (time_val - traj->start_time) / traj->duration;
        double inv_dur = 1.0 / traj->duration;
        double dh00 = hermite_dh00(s);
        double dh10 = hermite_dh10(s);
        double dh01 = hermite_dh01(s);
        double dh11 = hermite_dh11(s);
        for (int i = 0; i < nc; i++) {
            out[i] = (dh00 * traj->start.position[i] + dh10 * traj->m0[i] + dh01 * traj->end.position[i] + dh11 * traj->m1[i]) * inv_dur;
        }
    }

    for (int i = nc; i < 3; i++) out[i] = 0.0;
    return TRUE;
} /* trajectory_get_speed() */

int trajectory_get_acceleration(TRAJECTORY* traj, double time_val, VECTOR3D out) {
    __n_assert(traj, return FALSE);
    __n_assert(out, return FALSE);

    trajectory_load_segment(traj, time_val);
    int nc = traj->nb_components;

    if (traj->duration <= 0.0) {
        for (int i = 0; i < nc; i++) out[i] = traj->start.acceleration[i];
        for (int i = nc; i < 3; i++) out[i] = 0.0;
        return TRUE;
    }

    if (time_val < traj->start_time || time_val > traj->end_time) {
        /* extrapolation: constant acceleration from nearest endpoint */
        PHYSICS* ref = (time_val < traj->start_time) ? &traj->start : &traj->end;
        for (int i = 0; i < nc; i++) {
            out[i] = ref->acceleration[i];
        }
    } else {
        double s = (time_val - traj->start_time) / traj->duration;
        double inv_dur2 = 1.0 / (traj->duration * traj->duration);
        double d2h00 = hermite_d2h00(s);
        double d2h10 = hermite_d2h10(s);
        double d2h01 = hermite_d2h01(s);
        double d2h11 = hermite_d2h11(s);
        for (int i = 0; i < nc; i++) {
            out[i] = (d2h00 * traj->start.position[i] + d2h10 * traj->m0[i] + d2h01 * traj->end.position[i] + d2h11 * traj->m1[i]) * inv_dur2;
        }
    }

    for (int i = nc; i < 3; i++) out[i] = 0.0;
    return TRUE;
} /* trajectory_get_acceleration() */

int trajectory_get_orientation(TRAJECTORY* traj, double time_val, VECTOR3D out) {
    __n_assert(traj, return FALSE);
    __n_assert(out, return FALSE);

    trajectory_load_segment(traj, time_val);
    int nc = traj->nb_components;

    if (traj->duration <= 0.0) {
        for (int i = 0; i < nc; i++) out[i] = traj->start.orientation[i];
        for (int i = nc; i < 3; i++) out[i] = 0.0;
        return TRUE;
    }

    if (time_val < traj->start_time) {
        double dt = time_val - traj->start_time;
        for (int i = 0; i < nc; i++) {
            out[i] = traj->start.orientation[i] + traj->start.angular_speed[i] * dt + 0.5 * traj->start.angular_acceleration[i] * dt * dt;
        }
    } else if (time_val > traj->end_time) {
        double dt = time_val - traj->end_time;
        for (int i = 0; i < nc; i++) {
            out[i] = traj->end.orientation[i] + traj->end.angular_speed[i] * dt + 0.5 * traj->end.angular_acceleration[i] * dt * dt;
        }
    } else {
        double s = (time_val - traj->start_time) / traj->duration;
        double h00 = hermite_h00(s);
        double h10 = hermite_h10(s);
        double h01 = hermite_h01(s);
        double h11 = hermite_h11(s);
        for (int i = 0; i < nc; i++) {
            out[i] = h00 * traj->start.orientation[i] + h10 * traj->m0_orient[i] + h01 * traj->end.orientation[i] + h11 * traj->m1_orient[i];
        }
    }

    for (int i = nc; i < 3; i++) out[i] = 0.0;
    return TRUE;
} /* trajectory_get_orientation() */

int trajectory_add_point(TRAJECTORY* traj, const PHYSICS* state, double time_val) {
    __n_assert(traj, return FALSE);
    __n_assert(state, return FALSE);

    /* enforce chronological order */
    if (traj->nb_points > 0 && time_val <= traj->points[traj->nb_points - 1].time_val) {
        n_log(LOG_ERR, "time_val (%g) must be greater than last point time (%g)",
              time_val, traj->points[traj->nb_points - 1].time_val);
        return FALSE;
    }

    /* grow array if needed */
    if (traj->nb_points >= traj->nb_points_allocated) {
        int new_size = (traj->nb_points_allocated == 0) ? 8 : traj->nb_points_allocated * 2;
        __n_assert(new_size > 0, return FALSE);
        if (!Realloc(traj->points, TRAJECTORY_POINT, (size_t)new_size)) {
            return FALSE;
        }
        traj->nb_points_allocated = new_size;
    }

    memcpy(&traj->points[traj->nb_points].state, state, sizeof(PHYSICS));
    traj->points[traj->nb_points].time_val = time_val;
    traj->nb_points++;

    /* when we reach 2 points, load the first segment */
    if (traj->nb_points == 2) {
        traj->current_segment = -1; /* force reload */
        trajectory_load_segment(traj, traj->points[0].time_val);
    }

    return TRUE;
} /* trajectory_add_point() */

int trajectory_clear_points(TRAJECTORY* traj) {
    __n_assert(traj, return FALSE);

    traj->nb_points = 0;
    traj->current_segment = -1;
    memset(&traj->start, 0, sizeof(PHYSICS));
    memset(&traj->end, 0, sizeof(PHYSICS));
    traj->start_time = 0.0;
    traj->end_time = 0.0;
    traj->duration = 0.0;

    return TRUE;
} /* trajectory_clear_points() */

double trajectory_distance(TRAJECTORY* traj, double time_a, double time_b, int steps) {
    __n_assert(traj, return -1.0);

    if (steps < 1) steps = 1;

    double dist = 0.0;
    double dt = (time_b - time_a) / (double)steps;
    VECTOR3D prev, curr;
    /* nb_components is constrained to TRAJECTORY_2D (2) or TRAJECTORY_3D (3)
     * at construction time. Clamp to the VECTOR3D extent so the static
     * analyzer can see the bound and the per-component loop below stays
     * safely inside the [0,3) array range. */
    int nc = traj->nb_components;
    if (nc < 2) nc = 2;
    if (nc > 3) nc = 3;

    trajectory_get_position(traj, time_a, prev);

    for (int step = 1; step <= steps; step++) {
        double t = time_a + dt * (double)step;
        trajectory_get_position(traj, t, curr);

        double seg = 0.0;
        for (int i = 0; i < nc; i++) {
            double d = curr[i] - prev[i];
            seg += d * d;
        }
        dist += sqrt(seg);

        for (int i = 0; i < nc; i++) prev[i] = curr[i];
    }

    return dist;
} /* trajectory_distance() */