n_trajectory.c

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
 */
 
#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() */