examples/ex_trajectory.c

Structures

	Name
struct	WAYPOINT3D

Functions

	Name
void	update_3d_logic(void )
void	trail_push_3d(float x, float y)
void	trail_push_2d(float x, float y)
void	reset_2d(void )
void	rebuild_2d_trajectory(void )
void	project_3d(double x, double y, double z, float * sx, float * sy)
int	main(int argc, char * argv[])
void	init_physics_3d(PHYSICS * p, double px, double py, double pz, double vx, double vy, double vz)
void	init_physics_2d(PHYSICS * p, double px, double py, double vx, double vy)
void	init_3d(void )
void	handle_click_2d(float mx, float my)
void	draw_trail(float trail[][2], int count, int head, ALLEGRO_COLOR base_color)
void	draw_ground_grid(void )
void	draw_full_path_3d(void )
void	draw_full_path_2d(void )
void	draw_arrow(float x1, float y1, float x2, float y2, ALLEGRO_COLOR color, float thickness)
void	draw_3d_panel(ALLEGRO_FONT * font)
void	draw_3d_axes(ALLEGRO_FONT * font)
void	draw_2d_panel(ALLEGRO_FONT * font)

Attributes

	Name
double[50]	wp2d_y
double[50]	wp2d_x
double[50]	wp2d_t
int	wp2d_count
const WAYPOINT3D[7]	waypoints_3d
TRAJECTORY *	traj_3d
TRAJECTORY *	traj_2d
int	trail_3d_head
int	trail_3d_count
float[300][2]	trail_3d
int	trail_2d_head
int	trail_2d_count
float[300][2]	trail_2d
double	time_3d_end
double	time_3d
double	time_2d
int	running_2d
const double	WP3D_DUR
const double	WP2D_SPEED

Defines

	Name
	WIDTH
	PANEL_W
	NUM_WP3D
	M_PI
	MAX_WP2D
	MAX_TRAIL
	HEIGHT
	FPS
	DT
	ALLEGRO_UNSTABLE

Functions Documentation

function update_3d_logic

static void update_3d_logic(
    void 
)

function trail_push_3d

static void trail_push_3d(
    float x,
    float y
)

function trail_push_2d

static void trail_push_2d(
    float x,
    float y
)

function reset_2d

static void reset_2d(
    void 
)

function rebuild_2d_trajectory

static void rebuild_2d_trajectory(
    void 
)

function project_3d

static void project_3d(
    double x,
    double y,
    double z,
    float * sx,
    float * sy
)

function main

int main(
    int argc,
    char * argv[]
)

function init_physics_3d

static void init_physics_3d(
    PHYSICS * p,
    double px,
    double py,
    double pz,
    double vx,
    double vy,
    double vz
)

function init_physics_2d

static void init_physics_2d(
    PHYSICS * p,
    double px,
    double py,
    double vx,
    double vy
)

function init_3d

static void init_3d(
    void 
)

function handle_click_2d

static void handle_click_2d(
    float mx,
    float my
)

function draw_trail

static void draw_trail(
    float trail[][2],
    int count,
    int head,
    ALLEGRO_COLOR base_color
)

function draw_ground_grid

static void draw_ground_grid(
    void 
)

function draw_full_path_3d

static void draw_full_path_3d(
    void 
)

function draw_full_path_2d

static void draw_full_path_2d(
    void 
)

function draw_arrow

static void draw_arrow(
    float x1,
    float y1,
    float x2,
    float y2,
    ALLEGRO_COLOR color,
    float thickness
)

function draw_3d_panel

static void draw_3d_panel(
    ALLEGRO_FONT * font
)

function draw_3d_axes

static void draw_3d_axes(
    ALLEGRO_FONT * font
)

function draw_2d_panel

static void draw_2d_panel(
    ALLEGRO_FONT * font
)

Attributes Documentation

variable wp2d_y

static double[50] wp2d_y;

variable wp2d_x

static double[50] wp2d_x;

variable wp2d_t

static double[50] wp2d_t;

variable wp2d_count

static int wp2d_count = 0;

variable waypoints_3d

static const WAYPOINT3D[7] waypoints_3d                                      = {
    {0.0, 0.0, 0.0, 2.0, 1.0, 0.5},
    {3.0, 2.0, 1.0, 1.0, 1.5, -0.5},
    {4.0, 4.0, -1.0, -1.0, 1.0, -1.0},
    {1.0, 5.0, -2.0, -2.0, 0.0, 0.5},
    {-2.0, 3.0, 0.0, -1.0, -1.5, 1.0},
    {-1.0, 1.0, 1.5, 1.0, -1.0, 0.0},
    {0.0, 0.0, 0.0, 2.0, 1.0, 0.5}, 
};

variable traj_3d

static TRAJECTORY * traj_3d = NULL;

variable traj_2d

static TRAJECTORY * traj_2d = NULL;

variable trail_3d_head

static int trail_3d_head = 0;

variable trail_3d_count

static int trail_3d_count = 0;

variable trail_3d

static float[300][2] trail_3d;

variable trail_2d_head

static int trail_2d_head = 0;

variable trail_2d_count

static int trail_2d_count = 0;

variable trail_2d

static float[300][2] trail_2d;

variable time_3d_end

static double time_3d_end = 0.0;

variable time_3d

static double time_3d = 0.0;

variable time_2d

static double time_2d = 0.0;

variable running_2d

static int running_2d = 0;

variable WP3D_DUR

static const double WP3D_DUR = 4.0;

variable WP2D_SPEED

static const double WP2D_SPEED = 80.0;

Macros Documentation

define WIDTH

#define WIDTH 800

define PANEL_W

#define PANEL_W 400

define NUM_WP3D

#define NUM_WP3D 7

define M_PI

#define M_PI 3.14159265358979323846

define MAX_WP2D

#define MAX_WP2D 50

define MAX_TRAIL

#define MAX_TRAIL 300

define HEIGHT

#define HEIGHT 600

define FPS

#define FPS 60.0

define DT

#define DT (1.0 / FPS)

define ALLEGRO_UNSTABLE

#define ALLEGRO_UNSTABLE 1

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


#define ALLEGRO_UNSTABLE 1

#define WIDTH 800
#define HEIGHT 600
#define PANEL_W 400
#define FPS 60.0
#define DT (1.0 / FPS)
#define MAX_TRAIL 300

#include <stdio.h>
#include <string.h>
#include <math.h>

#include "nilorea/n_common.h"
#include "nilorea/n_log.h"
#include "nilorea/n_trajectory.h"

#include <allegro5/allegro.h>
#include <allegro5/allegro_primitives.h>
#include <allegro5/allegro_font.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/* 2D Demo State */
static TRAJECTORY* traj_2d = NULL;
static double time_2d = 0.0;
static int running_2d = 0;

/* raw waypoint storage for Catmull-Rom velocity computation */
#define MAX_WP2D 50
static double wp2d_x[MAX_WP2D], wp2d_y[MAX_WP2D], wp2d_t[MAX_WP2D];
static int wp2d_count = 0;
static const double WP2D_SPEED = 80.0; /* pixels/sec reference speed */

static float trail_2d[MAX_TRAIL][2];
static int trail_2d_count = 0;
static int trail_2d_head = 0;

/* 3D Demo State */
static TRAJECTORY* traj_3d = NULL;
static double time_3d = 0.0;
static double time_3d_end = 0.0;

static float trail_3d[MAX_TRAIL][2];
static int trail_3d_count = 0;
static int trail_3d_head = 0;

/* 3D waypoints forming a closed loop (last = first for seamless looping) */
typedef struct {
    double x, y, z, vx, vy, vz;
} WAYPOINT3D;

#define NUM_WP3D 7
static const WAYPOINT3D waypoints_3d[NUM_WP3D] = {
    {0.0, 0.0, 0.0, 2.0, 1.0, 0.5},
    {3.0, 2.0, 1.0, 1.0, 1.5, -0.5},
    {4.0, 4.0, -1.0, -1.0, 1.0, -1.0},
    {1.0, 5.0, -2.0, -2.0, 0.0, 0.5},
    {-2.0, 3.0, 0.0, -1.0, -1.5, 1.0},
    {-1.0, 1.0, 1.5, 1.0, -1.0, 0.0},
    {0.0, 0.0, 0.0, 2.0, 1.0, 0.5}, /* loop back to start */
};
static const double WP3D_DUR = 4.0; /* seconds per 3D segment */

/* Helpers */

static void init_physics_2d(PHYSICS* p, double px, double py, double vx, double vy) {
    memset(p, 0, sizeof(PHYSICS));
    VECTOR3D_SET(p->position, px, py, 0.0);
    VECTOR3D_SET(p->speed, vx, vy, 0.0);
}

static void init_physics_3d(PHYSICS* p, double px, double py, double pz, double vx, double vy, double vz) {
    memset(p, 0, sizeof(PHYSICS));
    VECTOR3D_SET(p->position, px, py, pz);
    VECTOR3D_SET(p->speed, vx, vy, vz);
}

static void trail_push_2d(float x, float y) {
    trail_2d[trail_2d_head][0] = x;
    trail_2d[trail_2d_head][1] = y;
    trail_2d_head = (trail_2d_head + 1) % MAX_TRAIL;
    if (trail_2d_count < MAX_TRAIL) trail_2d_count++;
}

static void trail_push_3d(float x, float y) {
    trail_3d[trail_3d_head][0] = x;
    trail_3d[trail_3d_head][1] = y;
    trail_3d_head = (trail_3d_head + 1) % MAX_TRAIL;
    if (trail_3d_count < MAX_TRAIL) trail_3d_count++;
}

/* 3D Projection (Cabinet) */

static void project_3d(double x, double y, double z, float* sx, float* sy) {
    double scale = 30.0;
    double cx = (double)PANEL_W + (double)PANEL_W / 2.0;
    double cy = (double)HEIGHT / 2.0;
    double angle = M_PI / 4.0;
    *sx = (float)(cx + x * scale + z * 0.5 * cos(angle) * scale);
    *sy = (float)(cy - y * scale - z * 0.5 * sin(angle) * scale);
}

/* Drawing Utilities */

static void draw_arrow(float x1, float y1, float x2, float y2, ALLEGRO_COLOR color, float thickness) {
    al_draw_line(x1, y1, x2, y2, color, thickness);
    float dx = x2 - x1, dy = y2 - y1;
    float len = sqrtf(dx * dx + dy * dy);
    if (len < 2.0f) return;
    float ux = dx / len, uy = dy / len;
    float px = -uy * 4.0f, py = ux * 4.0f;
    float bx = x2 - ux * 8.0f, by = y2 - uy * 8.0f;
    al_draw_filled_triangle(x2, y2, bx + px, by + py,
                            bx - px, by - py, color);
}

static void draw_trail(float trail[][2], int count, int head, ALLEGRO_COLOR base_color) {
    if (count < 2) return;
    unsigned char r, g, b;
    al_unmap_rgb(base_color, &r, &g, &b);
    for (int i = 1; i < count; i++) {
        int i0 = (head - count + i - 1 + MAX_TRAIL) % MAX_TRAIL;
        int i1 = (head - count + i + MAX_TRAIL) % MAX_TRAIL;
        float alpha = (float)i / (float)count;
        ALLEGRO_COLOR c = al_map_rgba(
            (unsigned char)(r * alpha),
            (unsigned char)(g * alpha),
            (unsigned char)(b * alpha),
            (unsigned char)(alpha * 200));
        al_draw_line(trail[i0][0], trail[i0][1],
                     trail[i1][0], trail[i1][1], c, 2);
    }
}

/* 2D Demo: Init / Reset / Click / Rebuild / Draw */

static void rebuild_2d_trajectory(void) {
    trajectory_clear_points(traj_2d);
    if (wp2d_count < 2) return;

    for (int i = 0; i < wp2d_count; i++) {
        double vx, vy;

        if (i == 0) {
            /* forward difference */
            double dt = wp2d_t[1] - wp2d_t[0];
            vx = (wp2d_x[1] - wp2d_x[0]) / dt;
            vy = (wp2d_y[1] - wp2d_y[0]) / dt;
        } else if (i == wp2d_count - 1) {
            /* backward difference */
            double dt = wp2d_t[i] - wp2d_t[i - 1];
            vx = (wp2d_x[i] - wp2d_x[i - 1]) / dt;
            vy = (wp2d_y[i] - wp2d_y[i - 1]) / dt;
        } else {
            /* Catmull-Rom: central difference */
            double dt = wp2d_t[i + 1] - wp2d_t[i - 1];
            vx = (wp2d_x[i + 1] - wp2d_x[i - 1]) / dt;
            vy = (wp2d_y[i + 1] - wp2d_y[i - 1]) / dt;
        }

        PHYSICS state;
        init_physics_2d(&state, wp2d_x[i], wp2d_y[i], vx, vy);
        trajectory_add_point(traj_2d, &state, wp2d_t[i]);
    }

    running_2d = 1;
}

static void reset_2d(void) {
    if (traj_2d) trajectory_delete(&traj_2d);
    traj_2d = trajectory_new(TRAJECTORY_2D);
    time_2d = 0.0;
    running_2d = 0;
    wp2d_count = 0;
    trail_2d_count = 0;
    trail_2d_head = 0;
}

static void handle_click_2d(float mx, float my) {
    if (mx >= PANEL_W || wp2d_count >= MAX_WP2D) return;

    if (wp2d_count == 0) {
        wp2d_x[0] = (double)mx;
        wp2d_y[0] = (double)my;
        wp2d_t[0] = 0.0;
        wp2d_count = 1;
        return;
    }

    /* compute time for new waypoint based on distance */
    double dx = (double)mx - wp2d_x[wp2d_count - 1];
    double dy = (double)my - wp2d_y[wp2d_count - 1];
    double dist = sqrt(dx * dx + dy * dy);
    if (dist < 10.0) return;

    double dur = dist / WP2D_SPEED;
    if (dur < 0.3) dur = 0.3;

    wp2d_x[wp2d_count] = (double)mx;
    wp2d_y[wp2d_count] = (double)my;
    wp2d_t[wp2d_count] = wp2d_t[wp2d_count - 1] + dur;
    wp2d_count++;

    rebuild_2d_trajectory();
}

static void draw_full_path_2d(void) {
    if (!traj_2d || traj_2d->nb_points < 2) return;

    double t0 = traj_2d->points[0].time_val;
    double t1 = traj_2d->points[traj_2d->nb_points - 1].time_val;
    int total_steps = (traj_2d->nb_points - 1) * 30;

    VECTOR3D pos;
    trajectory_get_position(traj_2d, t0, pos);
    float px = (float)pos[0], py = (float)pos[1];

    for (int i = 1; i <= total_steps; i++) {
        double t = t0 + (t1 - t0) * (double)i / (double)total_steps;
        trajectory_get_position(traj_2d, t, pos);
        float nx = (float)pos[0], ny = (float)pos[1];
        al_draw_line(px, py, nx, ny,
                     al_map_rgba(0, 200, 255, 180), 2);
        px = nx;
        py = ny;
    }

    /* extrapolation tail: base length on last segment, not total path */
    double last_seg_dur = traj_2d->points[traj_2d->nb_points - 1].time_val - traj_2d->points[traj_2d->nb_points - 2].time_val;
    double ext = last_seg_dur * 0.5;
    if (ext < 0.3) ext = 0.3;
    for (int i = 1; i <= 20; i++) {
        double t = t1 + ext * (double)i / 20.0;
        trajectory_get_position(traj_2d, t, pos);
        float nx = (float)pos[0], ny = (float)pos[1];
        if (i % 2 == 0)
            al_draw_line(px, py, nx, ny,
                         al_map_rgba(255, 255, 0, 120), 1);
        px = nx;
        py = ny;
    }
}

static void draw_2d_panel(ALLEGRO_FONT* font) {
    al_draw_filled_rectangle(0, 0, PANEL_W, HEIGHT,
                             al_map_rgb(15, 15, 30));

    ALLEGRO_COLOR grid = al_map_rgba(35, 35, 55, 255);
    for (int x = 0; x < PANEL_W; x += 40)
        al_draw_line((float)x, 0, (float)x, HEIGHT, grid, 1);
    for (int y = 0; y < HEIGHT; y += 40)
        al_draw_line(0, (float)y, PANEL_W, (float)y, grid, 1);

    al_draw_text(font, al_map_rgb(220, 220, 220), 5, 5, 0,
                 "2D Multi-Waypoint Path");

    /* draw full spline path */
    draw_full_path_2d();

    /* draw all waypoint markers */
    for (int i = 0; i < wp2d_count; i++) {
        float wx = (float)wp2d_x[i], wy = (float)wp2d_y[i];
        ALLEGRO_COLOR wc;
        if (i == 0)
            wc = al_map_rgb(0, 200, 0);
        else if (i == wp2d_count - 1)
            wc = al_map_rgb(200, 0, 0);
        else
            wc = al_map_rgb(100, 150, 255);
        al_draw_filled_circle(wx, wy, 5, wc);
        al_draw_circle(wx, wy, 5, al_map_rgb(255, 255, 255), 1);

        char num[16];
        snprintf(num, sizeof(num), "%d", i);
        al_draw_text(font, al_map_rgb(200, 200, 200),
                     wx + 7, wy - 6, 0, num);
    }

    if (running_2d && traj_2d && traj_2d->nb_points >= 2) {
        /* trail */
        draw_trail(trail_2d, trail_2d_count, trail_2d_head,
                   al_map_rgb(100, 180, 255));

        /* current object */
        trajectory_compute(traj_2d, time_2d);
        float ox = (float)traj_2d->current.position[0];
        float oy = (float)traj_2d->current.position[1];

        al_draw_filled_circle(ox, oy, 8,
                              al_map_rgb(255, 180, 0));
        al_draw_circle(ox, oy, 8,
                       al_map_rgb(255, 255, 255), 2);

        /* velocity arrow */
        float vs = 0.15f;
        draw_arrow(ox, oy,
                   ox + (float)traj_2d->current.speed[0] * vs,
                   oy + (float)traj_2d->current.speed[1] * vs,
                   al_map_rgb(255, 255, 100), 1);

        /* info text */
        char buf[128];
        const char* mode = "???";
        if (traj_2d->mode == TRAJECTORY_INTERP)
            mode = "INTERP";
        else if (traj_2d->mode == TRAJECTORY_EXTRAP)
            mode = "EXTRAP";
        else if (traj_2d->mode == TRAJECTORY_BEFORE)
            mode = "BEFORE";

        snprintf(buf, sizeof(buf), "t=%.2f  seg=%d/%d  [%s]",
                 time_2d, traj_2d->current_segment,
                 traj_2d->nb_points - 1, mode);
        al_draw_text(font, al_map_rgb(200, 200, 200), 5, 18, 0, buf);
        snprintf(buf, sizeof(buf), "pos=(%.0f,%.0f) vel=(%.0f,%.0f) wp=%d",
                 traj_2d->current.position[0],
                 traj_2d->current.position[1],
                 traj_2d->current.speed[0],
                 traj_2d->current.speed[1],
                 wp2d_count);
        al_draw_text(font, al_map_rgb(200, 200, 200), 5, 34, 0, buf);
    } else {
        const char* msg = (wp2d_count == 0)
                              ? "Click to place waypoints"
                              : "Click to add more waypoints";
        al_draw_text(font, al_map_rgb(150, 150, 150),
                     PANEL_W / 2, 18,
                     ALLEGRO_ALIGN_CENTER, msg);
    }

    /* legend */
    al_draw_line(5, HEIGHT - 60, 25, HEIGHT - 60,
                 al_map_rgba(0, 200, 255, 180), 2);
    al_draw_text(font, al_map_rgb(160, 160, 160),
                 30, HEIGHT - 66, 0, "Interpolation");
    al_draw_line(5, HEIGHT - 46, 15, HEIGHT - 46,
                 al_map_rgba(255, 255, 0, 120), 1);
    al_draw_line(18, HEIGHT - 46, 25, HEIGHT - 46,
                 al_map_rgba(255, 255, 0, 120), 1);
    al_draw_text(font, al_map_rgb(160, 160, 160),
                 30, HEIGHT - 52, 0, "Extrapolation");

    al_draw_text(font, al_map_rgb(90, 90, 90), 5, HEIGHT - 15, 0,
                 "[R] Reset   [ESC] Quit");
}

/* 3D Demo: Init / Update / Draw */

static void init_3d(void) {
    traj_3d = trajectory_new(TRAJECTORY_3D);
    time_3d = 0.0;
    trail_3d_count = 0;
    trail_3d_head = 0;

    /* add all waypoints to the trajectory */
    double t = 0.0;
    for (int i = 0; i < NUM_WP3D; i++) {
        const WAYPOINT3D* w = &waypoints_3d[i];
        PHYSICS state;
        init_physics_3d(&state, w->x, w->y, w->z,
                        w->vx, w->vy, w->vz);
        trajectory_add_point(traj_3d, &state, t);
        t += WP3D_DUR;
    }
    time_3d_end = t - WP3D_DUR; /* time of last waypoint */
}

static void update_3d_logic(void) {
    if (!traj_3d || traj_3d->nb_points < 2) return;

    time_3d += DT;

    /* loop when past the last waypoint */
    if (time_3d >= time_3d_end) {
        time_3d -= time_3d_end;
        trail_3d_count = 0;
        trail_3d_head = 0;
    }

    trajectory_compute(traj_3d, time_3d);
    float sx, sy;
    project_3d(traj_3d->current.position[0],
               traj_3d->current.position[1],
               traj_3d->current.position[2], &sx, &sy);
    trail_push_3d(sx, sy);
}

static void draw_3d_axes(ALLEGRO_FONT* font) {
    float ox, oy, ax, ay;
    project_3d(0, 0, 0, &ox, &oy);

    project_3d(2.5, 0, 0, &ax, &ay);
    draw_arrow(ox, oy, ax, ay, al_map_rgb(200, 60, 60), 2);
    al_draw_text(font, al_map_rgb(200, 60, 60),
                 ax + 4, ay - 6, 0, "X");

    project_3d(0, 2.5, 0, &ax, &ay);
    draw_arrow(ox, oy, ax, ay, al_map_rgb(60, 200, 60), 2);
    al_draw_text(font, al_map_rgb(60, 200, 60),
                 ax + 4, ay - 6, 0, "Y");

    project_3d(0, 0, 2.5, &ax, &ay);
    draw_arrow(ox, oy, ax, ay, al_map_rgb(60, 60, 200), 2);
    al_draw_text(font, al_map_rgb(60, 60, 200),
                 ax + 4, ay - 6, 0, "Z");
}

static void draw_ground_grid(void) {
    ALLEGRO_COLOR gc = al_map_rgba(45, 45, 45, 128);
    for (int i = -4; i <= 4; i++) {
        float x1, y1, x2, y2;
        project_3d((double)i, 0, -4, &x1, &y1);
        project_3d((double)i, 0, 4, &x2, &y2);
        al_draw_line(x1, y1, x2, y2, gc, 1);
        project_3d(-4, 0, (double)i, &x1, &y1);
        project_3d(4, 0, (double)i, &x2, &y2);
        al_draw_line(x1, y1, x2, y2, gc, 1);
    }
}

static void draw_full_path_3d(void) {
    if (!traj_3d || traj_3d->nb_points < 2) return;

    double t0 = traj_3d->points[0].time_val;
    double t1 = traj_3d->points[traj_3d->nb_points - 1].time_val;
    int total_steps = (traj_3d->nb_points - 1) * 30;

    VECTOR3D pos;
    trajectory_get_position(traj_3d, t0, pos);
    float px, py;
    project_3d(pos[0], pos[1], pos[2], &px, &py);

    for (int i = 1; i <= total_steps; i++) {
        double t = t0 + (t1 - t0) * (double)i / (double)total_steps;
        trajectory_get_position(traj_3d, t, pos);
        float nx, ny;
        project_3d(pos[0], pos[1], pos[2], &nx, &ny);
        al_draw_line(px, py, nx, ny,
                     al_map_rgba(255, 100, 255, 140), 2);
        px = nx;
        py = ny;
    }
}

static void draw_3d_panel(ALLEGRO_FONT* font) {
    al_draw_filled_rectangle(PANEL_W, 0, WIDTH, HEIGHT,
                             al_map_rgb(18, 18, 22));

    al_draw_text(font, al_map_rgb(220, 220, 220),
                 PANEL_W + 5, 5, 0, "3D Multi-Waypoint Loop");

    draw_ground_grid();
    draw_3d_axes(font);

    if (!traj_3d || traj_3d->nb_points < 2) return;

    /* draw full looping path */
    draw_full_path_3d();

    /* draw waypoint markers */
    for (int i = 0; i < NUM_WP3D; i++) {
        float sx, sy;
        project_3d(waypoints_3d[i].x, waypoints_3d[i].y,
                   waypoints_3d[i].z, &sx, &sy);
        ALLEGRO_COLOR c = (i == traj_3d->current_segment ||
                           i == traj_3d->current_segment + 1)
                              ? al_map_rgb(255, 80, 80)
                              : al_map_rgb(100, 100, 200);
        al_draw_filled_circle(sx, sy, 4, c);
        al_draw_circle(sx, sy, 4, al_map_rgb(200, 200, 200), 1);

        char num[16];
        snprintf(num, sizeof(num), "%d", i);
        al_draw_text(font, al_map_rgb(180, 180, 180),
                     sx + 6, sy - 6, 0, num);
    }

    /* trail */
    draw_trail(trail_3d, trail_3d_count, trail_3d_head,
               al_map_rgb(200, 120, 255));

    /* current object */
    trajectory_compute(traj_3d, time_3d);
    float obj_sx, obj_sy;
    project_3d(traj_3d->current.position[0],
               traj_3d->current.position[1],
               traj_3d->current.position[2],
               &obj_sx, &obj_sy);

    /* vertical drop line (depth cue) */
    float gnd_sx, gnd_sy;
    project_3d(traj_3d->current.position[0], 0,
               traj_3d->current.position[2],
               &gnd_sx, &gnd_sy);
    al_draw_line(gnd_sx, gnd_sy, obj_sx, obj_sy,
                 al_map_rgba(100, 100, 100, 80), 1);
    al_draw_filled_circle(gnd_sx, gnd_sy, 2,
                          al_map_rgba(100, 100, 100, 60));

    /* object */
    al_draw_filled_circle(obj_sx, obj_sy, 7,
                          al_map_rgb(255, 200, 50));
    al_draw_circle(obj_sx, obj_sy, 7,
                   al_map_rgb(255, 255, 255), 2);

    /* velocity arrow */
    double vs3 = 0.4;
    float vel_sx, vel_sy;
    project_3d(traj_3d->current.position[0] +
                   traj_3d->current.speed[0] * vs3,
               traj_3d->current.position[1] +
                   traj_3d->current.speed[1] * vs3,
               traj_3d->current.position[2] +
                   traj_3d->current.speed[2] * vs3,
               &vel_sx, &vel_sy);
    draw_arrow(obj_sx, obj_sy, vel_sx, vel_sy,
               al_map_rgb(255, 255, 100), 1);

    /* info text */
    char buf[128];
    snprintf(buf, sizeof(buf), "t=%.2f  seg=%d/%d  wp=%d",
             time_3d, traj_3d->current_segment,
             traj_3d->nb_points - 1, traj_3d->nb_points);
    al_draw_text(font, al_map_rgb(200, 200, 200),
                 PANEL_W + 5, 18, 0, buf);
    snprintf(buf, sizeof(buf), "pos=(%.1f, %.1f, %.1f)",
             traj_3d->current.position[0],
             traj_3d->current.position[1],
             traj_3d->current.position[2]);
    al_draw_text(font, al_map_rgb(200, 200, 200),
                 PANEL_W + 5, 34, 0, buf);
    snprintf(buf, sizeof(buf), "vel=(%.1f, %.1f, %.1f)",
             traj_3d->current.speed[0],
             traj_3d->current.speed[1],
             traj_3d->current.speed[2]);
    al_draw_text(font, al_map_rgb(200, 200, 200),
                 PANEL_W + 5, 50, 0, buf);
}

/* Main */

int main(int argc, char* argv[]) {
    (void)argc;
    set_log_level(LOG_ERR);

    n_log(LOG_NOTICE, "%s is starting ...", argv[0]);

    if (!al_init()) {
        n_abort("Could not init Allegro.\n");
    }
    if (!al_init_primitives_addon()) {
        n_abort("Unable to initialize primitives addon\n");
    }
    if (!al_init_font_addon()) {
        n_abort("Unable to initialize font addon\n");
    }
    if (!al_install_keyboard()) {
        n_abort("Unable to initialize keyboard handler\n");
    }
    if (!al_install_mouse()) {
        n_abort("Unable to initialize mouse handler\n");
    }

    al_set_new_display_flags(ALLEGRO_OPENGL | ALLEGRO_WINDOWED);
    ALLEGRO_DISPLAY* display = al_create_display(WIDTH, HEIGHT);
    if (!display) {
        n_abort("Unable to create display\n");
    }
    al_set_window_title(display,
                        "Nilorea Trajectory API - Multi-Waypoint Demo");

    ALLEGRO_FONT* font = al_create_builtin_font();
    ALLEGRO_TIMER* timer = al_create_timer(1.0 / FPS);

    ALLEGRO_EVENT_QUEUE* event_queue = al_create_event_queue();
    al_register_event_source(event_queue,
                             al_get_display_event_source(display));
    al_register_event_source(event_queue,
                             al_get_timer_event_source(timer));
    al_register_event_source(event_queue,
                             al_get_keyboard_event_source());
    al_register_event_source(event_queue,
                             al_get_mouse_event_source());

    ALLEGRO_BITMAP* scrbuf = al_create_bitmap(WIDTH, HEIGHT);

    reset_2d();
    init_3d();

    al_start_timer(timer);

    int done = 0;
    int do_draw = 0;

    while (!done) {
        ALLEGRO_EVENT ev;
        al_wait_for_event(event_queue, &ev);

        if (ev.type == ALLEGRO_EVENT_DISPLAY_CLOSE) {
            done = 1;
        } else if (ev.type == ALLEGRO_EVENT_KEY_DOWN) {
            if (ev.keyboard.keycode == ALLEGRO_KEY_ESCAPE) {
                done = 1;
            } else if (ev.keyboard.keycode == ALLEGRO_KEY_R) {
                reset_2d();
            }
        } else if (ev.type == ALLEGRO_EVENT_MOUSE_BUTTON_DOWN) {
            if (ev.mouse.button == 1) {
                handle_click_2d((float)ev.mouse.x,
                                (float)ev.mouse.y);
            }
        } else if (ev.type == ALLEGRO_EVENT_DISPLAY_SWITCH_IN ||
                   ev.type == ALLEGRO_EVENT_DISPLAY_SWITCH_OUT) {
            al_clear_keyboard_state(display);
            al_flush_event_queue(event_queue);
        } else if (ev.type == ALLEGRO_EVENT_TIMER) {
            /* 2D logic */
            if (running_2d && traj_2d && traj_2d->nb_points >= 2) {
                time_2d += DT;
                trajectory_compute(traj_2d, time_2d);
                trail_push_2d(
                    (float)traj_2d->current.position[0],
                    (float)traj_2d->current.position[1]);
            }

            /* 3D logic */
            update_3d_logic();

            do_draw = 1;
        }

        if (do_draw && al_is_event_queue_empty(event_queue)) {
            al_set_target_bitmap(scrbuf);
            al_clear_to_color(al_map_rgb(0, 0, 0));

            draw_2d_panel(font);
            draw_3d_panel(font);

            al_draw_line(PANEL_W, 0, PANEL_W, HEIGHT,
                         al_map_rgb(80, 80, 80), 2);

            al_set_target_bitmap(al_get_backbuffer(display));
            al_draw_bitmap(scrbuf, 0, 0, 0);
            al_flip_display();
            do_draw = 0;
        }
    }

    trajectory_delete(&traj_2d);
    trajectory_delete(&traj_3d);
    al_destroy_bitmap(scrbuf);
    al_destroy_font(font);
    al_destroy_timer(timer);
    al_destroy_event_queue(event_queue);
    al_destroy_display(display);

    return 0;
}