libakgl/src/collision.c

/**
 * @file collision.c
 * @brief The narrowphase, and the only place libccd is visible.
 *
 * Nothing else in the tree includes `<ccd/ccd.h>`. The translation between
 * libakgl's shapes and libccd's support-function protocol is `static` here, so
 * changing narrowphase library is a change to one file and no header -- and so
 * that `akgl.pc` never has to name a dependency that is compiled in.
 */

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

#include <akerror.h>

#include <akgl/collision.h>
#include <akstdlib.h>
#include <akgl/collision_arena.h>
#include <akgl/error.h>
#include <akgl/actor.h>
#include <akgl/heap.h>
#include <akgl/tilemap.h>
#include <akgl/types.h>

#include <ccd/ccd.h>
#include <ccd/quat.h>
#include <ccd/vec3.h>

/**
 * @brief Ceiling on narrowphase iterations.
 *
 * libccd's own default, set by `CCD_INIT`, is `(unsigned long)-1` -- an
 * unbounded loop inside a frame. A pair that reaches this cap is reported as not
 * colliding, which is the same answer a missed collision gives, but it
 * terminates. Bounding the iterations also bounds what the arena can be asked
 * for, since the polytope grows with them.
 */
#define AKGL_COLLISION_MAX_ITERATIONS   100

/** @brief Below this, a normal is treated as having no length at all. */
#define AKGL_COLLISION_EPSILON          1e-6f

/**
 * @brief Nudge applied to a box's far edge before it is turned into tile indices.
 *
 * A thousandth of a pixel, and **not** #AKGL_COLLISION_EPSILON. That one is a
 * tolerance on a unit vector, where 1e-6 is meaningful; this one is subtracted
 * from a map coordinate, where it is not. `float` has about seven significant
 * digits, so at a coordinate of 144 the smallest representable step is around
 * 1.5e-5 and `144.0f - 1e-6f` is exactly 144.0f. The nudge does nothing, the far
 * edge lands on the next tile, and an actor standing flush on the ground reads
 * as inside it -- which makes every move it tries look blocked.
 *
 * The sidescroller example arrived at the same number for the same reason.
 */
#define AKGL_COLLISION_TILE_EPSILON     0.001f

/** @brief Tiles akgl_collision_settle will lift a shape before giving up. */
#define AKGL_COLLISION_SETTLE_STEPS     4

/**
 * @brief Fraction of the thinnest passable thickness one sub-step may cover.
 *
 * Half. Anything that moves less than half the thinnest thing it could pass
 * through cannot be on both sides of it between two samples.
 */
#define AKGL_COLLISION_SUBSTEP_FRACTION 0.5f

/**
 * @brief Hard ceiling on sub-steps per actor per step.
 *
 * A cost bound. Above roughly `MAX_SUBSTEPS * FRACTION * cellsize` per step an
 * actor can still pass through a wall -- about 1280 px/s on 16-pixel tiles at
 * the default step, which is a projectile and not a walker. The real answer is
 * a swept narrowphase; #AKGL_COLLISION_FLAG_BULLET reserves the bit for it.
 */
#define AKGL_COLLISION_MAX_SUBSTEPS     8

/** @brief One positioned shape, in the form libccd's callbacks read. */
typedef struct {
    uint8_t kind;       /**< AKGL_COLLISION_SHAPE_*. */
    ccd_vec3_t pos;     /**< World centre: owner position plus shape offset. */
    ccd_vec3_t half;    /**< Half-extents. `v[0]` doubles as the radius for a circle. */
} collision_ccdobj;

/**
 * @brief Furthest point of a shape in a given direction.
 *
 * The support function *is* the shape as far as libccd is concerned. One
 * function dispatching on kind rather than one per kind, because a pair may mix
 * kinds and `ccd_t` has no way to choose per object beyond this.
 *
 * There is no rotation, here or anywhere in this library yet -- `util.h` says so
 * and both example games depend on it. Adding it means rotating @p dir into the
 * shape's local frame at the top of this function and rotating the answer back,
 * and nothing else in the narrowphase changes. See `TODO.md`, "Actor rotation".
 */
static void collision_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *dest)
{
    const collision_ccdobj *shape = (const collision_ccdobj *)obj;
    ccd_real_t planar = 0.0;
    ccd_real_t len = 0.0;

    switch ( shape->kind ) {
    case AKGL_COLLISION_SHAPE_CIRCLE:
	/*
	 * A circle extruded along z is a cylinder, not a sphere. A sphere's caps
	 * curve away from the plane and would let a shape slide past a corner
	 * that a 2D game expects to catch.
	 */
	len = (ccdVec3X(dir) * ccdVec3X(dir)) + (ccdVec3Y(dir) * ccdVec3Y(dir));
	if ( len > AKGL_COLLISION_EPSILON ) {
	    len = CCD_SQRT(len);
	    planar = ccdVec3X(&shape->half) / len;
	    ccdVec3Set(dest, ccdVec3X(dir) * planar, ccdVec3Y(dir) * planar, CCD_ZERO);
	} else {
	    ccdVec3Set(dest, CCD_ZERO, CCD_ZERO, CCD_ZERO);
	}
	dest->v[2] = ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half);
	break;
    case AKGL_COLLISION_SHAPE_CAPSULE_X:
	// A segment on x with a circle of radius hy swept along it.
	ccdVec3Set(dest,
		   ccdSign(ccdVec3X(dir)) * (ccdVec3X(&shape->half) - ccdVec3Y(&shape->half)),
		   CCD_ZERO,
		   CCD_ZERO);
	len = (ccdVec3X(dir) * ccdVec3X(dir)) + (ccdVec3Y(dir) * ccdVec3Y(dir));
	if ( len > AKGL_COLLISION_EPSILON ) {
	    len = CCD_SQRT(len);
	    planar = ccdVec3Y(&shape->half) / len;
	    dest->v[0] += ccdVec3X(dir) * planar;
	    dest->v[1] += ccdVec3Y(dir) * planar;
	}
	dest->v[2] = ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half);
	break;
    case AKGL_COLLISION_SHAPE_CAPSULE_Y:
	ccdVec3Set(dest,
		   CCD_ZERO,
		   ccdSign(ccdVec3Y(dir)) * (ccdVec3Y(&shape->half) - ccdVec3X(&shape->half)),
		   CCD_ZERO);
	len = (ccdVec3X(dir) * ccdVec3X(dir)) + (ccdVec3Y(dir) * ccdVec3Y(dir));
	if ( len > AKGL_COLLISION_EPSILON ) {
	    len = CCD_SQRT(len);
	    planar = ccdVec3X(&shape->half) / len;
	    dest->v[0] += ccdVec3X(dir) * planar;
	    dest->v[1] += ccdVec3Y(dir) * planar;
	}
	dest->v[2] = ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half);
	break;
    default:
	// A box, and the fallback for anything unrecognised: the corner in the
	// direction's octant.
	ccdVec3Set(dest,
		   ccdSign(ccdVec3X(dir)) * ccdVec3X(&shape->half),
		   ccdSign(ccdVec3Y(dir)) * ccdVec3Y(&shape->half),
		   ccdSign(ccdVec3Z(dir)) * ccdVec3Z(&shape->half));
	break;
    }
    ccdVec3Add(dest, &shape->pos);
}

/**
 * @brief Centre of a shape.
 *
 * MPR requires this where GJK does not: `ccdMPRPenetration` calls `center1` and
 * `center2` unconditionally, so leaving either `NULL` is a null dereference on
 * the first contact rather than a degraded answer.
 */
static void collision_center(const void *obj, ccd_vec3_t *dest)
{
    const collision_ccdobj *shape = (const collision_ccdobj *)obj;

    ccdVec3Copy(dest, &shape->pos);
}

/** @brief Put a proxy into the form the support functions read. */
static void collision_to_ccd(akgl_CollisionProxy *proxy, collision_ccdobj *dest)
{
    dest->kind = proxy->shape.kind;
    ccdVec3Set(&dest->pos,
	       (ccd_real_t)(proxy->x + proxy->shape.ox),
	       (ccd_real_t)(proxy->y + proxy->shape.oy),
	       (ccd_real_t)(proxy->z + proxy->shape.oz));
    ccdVec3Set(&dest->half,
	       (ccd_real_t)proxy->shape.hx,
	       (ccd_real_t)proxy->shape.hy,
	       (ccd_real_t)proxy->shape.hz);
}

/** @brief Configure the solver. Identical for every query. */
static void collision_configure(ccd_t *ccd)
{
    CCD_INIT(ccd);
    ccd->support1 = collision_support;
    ccd->support2 = collision_support;
    ccd->center1 = collision_center;
    ccd->center2 = collision_center;
    ccd->max_iterations = AKGL_COLLISION_MAX_ITERATIONS;
}

/**
 * @brief Box against box, in closed form.
 *
 * Overlap on each axis, take the smallest, and the normal is that axis signed
 * away from @p b. Exact rather than converged, which matters for a resting
 * actor: an iterative solver answers `(0.0001, -0.99999, 0)` where this answers
 * `(0, -1, 0)`, and that difference accumulates into a slow sideways creep along
 * a floor.
 */
static akerr_ErrorContext *collision_box_box(akgl_CollisionProxy *a, akgl_CollisionProxy *b, akgl_Contact *dest, bool *hit)
{
    float32_t delta[3];
    float32_t overlap[3];
    float32_t acentre[3];
    float32_t bcentre[3];
    int axis = 0;
    int i = 0;

    PREPARE_ERROR(errctx);

    acentre[0] = a->x + a->shape.ox;
    acentre[1] = a->y + a->shape.oy;
    acentre[2] = a->z + a->shape.oz;
    bcentre[0] = b->x + b->shape.ox;
    bcentre[1] = b->y + b->shape.oy;
    bcentre[2] = b->z + b->shape.oz;

    delta[0] = bcentre[0] - acentre[0];
    delta[1] = bcentre[1] - acentre[1];
    delta[2] = bcentre[2] - acentre[2];

    overlap[0] = (a->shape.hx + b->shape.hx) - fabsf(delta[0]);
    overlap[1] = (a->shape.hy + b->shape.hy) - fabsf(delta[1]);
    overlap[2] = (a->shape.hz + b->shape.hz) - fabsf(delta[2]);

    *hit = false;
    for ( i = 0; i < 3; i++ ) {
	if ( overlap[i] <= 0.0f ) {
	    SUCCEED_RETURN(errctx);
	}
    }

    for ( i = 1; i < 3; i++ ) {
	if ( overlap[i] < overlap[axis] ) {
	    axis = i;
	}
    }

    dest->nx = 0.0f;
    dest->ny = 0.0f;
    dest->nz = 0.0f;
    /*
     * Away from b. If b's centre is to the right of a's, then a leaves to the
     * left and the normal is negative on that axis. A delta of exactly 0 means
     * the two are concentric here and either direction is as good; pick one
     * rather than emit a zero-length normal, which would be a wall that moves
     * nothing.
     */
    if ( axis == 0 ) {
	dest->nx = (delta[0] > 0.0f) ? -1.0f : 1.0f;
    } else if ( axis == 1 ) {
	dest->ny = (delta[1] > 0.0f) ? -1.0f : 1.0f;
    } else {
	dest->nz = (delta[2] > 0.0f) ? -1.0f : 1.0f;
    }
    dest->depth = overlap[axis];

    // Midpoint of the two centres. For two boxes that sits inside the overlap.
    dest->px = (acentre[0] + bcentre[0]) / 2.0f;
    dest->py = (acentre[1] + bcentre[1]) / 2.0f;
    dest->pz = (acentre[2] + bcentre[2]) / 2.0f;

    *hit = true;
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Flatten a normal into the xy plane, and rescue a degenerate one.
 *
 * See #AKGL_COLLISION_TEST_PLANAR. The fallback matters more than it looks: two
 * shapes at exactly the same centre have no planar direction to separate along,
 * and level authors put things on top of each other constantly. Answering with a
 * zero-length normal there would move an actor by nothing and report success,
 * which is a wall that does not stop anything.
 */
static void collision_flatten(akgl_CollisionProxy *a, akgl_CollisionProxy *b, akgl_Contact *dest)
{
    float32_t len = 0.0f;
    float32_t dx = 0.0f;
    float32_t dy = 0.0f;

    dest->nz = 0.0f;
    len = sqrtf((dest->nx * dest->nx) + (dest->ny * dest->ny));
    if ( len > AKGL_COLLISION_EPSILON ) {
	dest->nx = dest->nx / len;
	dest->ny = dest->ny / len;
	return;
    }

    // Nothing planar survived. Separate along whichever axis they overlap least.
    dx = (a->shape.hx + b->shape.hx) - fabsf((b->x + b->shape.ox) - (a->x + a->shape.ox));
    dy = (a->shape.hy + b->shape.hy) - fabsf((b->y + b->shape.oy) - (a->y + a->shape.oy));
    if ( dx <= dy ) {
	dest->nx = (((b->x + b->shape.ox) - (a->x + a->shape.ox)) > 0.0f) ? -1.0f : 1.0f;
	dest->ny = 0.0f;
	dest->depth = dx;
    } else {
	dest->nx = 0.0f;
	dest->ny = (((b->y + b->shape.oy) - (a->y + a->shape.oy)) > 0.0f) ? -1.0f : 1.0f;
	dest->depth = dy;
    }
}

akerr_ErrorContext *akgl_collision_test(akgl_CollisionProxy *a, akgl_CollisionProxy *b, uint32_t flags, akgl_Contact *dest, bool *hit)
{
    collision_ccdobj obja;
    collision_ccdobj objb;
    ccd_t ccd;
    ccd_real_t depth = 0.0;
    ccd_vec3_t dir;
    ccd_vec3_t pos;
    int result = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, a, AKERR_NULLPOINTER, "NULL first proxy reference");
    FAIL_ZERO_RETURN(errctx, b, AKERR_NULLPOINTER, "NULL second proxy reference");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "NULL contact reference");
    FAIL_ZERO_RETURN(errctx, hit, AKERR_NULLPOINTER, "NULL hit flag reference");

    memset(dest, 0x00, sizeof(akgl_Contact));
    dest->tilex = -1;
    dest->tiley = -1;
    dest->tilelayer = -1;
    *hit = false;

    if ( (a->shape.kind == AKGL_COLLISION_SHAPE_NONE) ||
	 (b->shape.kind == AKGL_COLLISION_SHAPE_NONE) ) {
	SUCCEED_RETURN(errctx);
    }

    /*
     * The bounds are already computed and already stored, so the cheapest
     * rejection in the system costs four comparisons and no arithmetic. Most
     * candidate pairs a broad phase hands over die right here.
     */
    if ( !SDL_HasRectIntersectionFloat(&a->bounds, &b->bounds) ) {
	SUCCEED_RETURN(errctx);
    }

    if ( (a->shape.kind == AKGL_COLLISION_SHAPE_BOX) &&
	 (b->shape.kind == AKGL_COLLISION_SHAPE_BOX) ) {
	PASS(errctx, collision_box_box(a, b, dest, hit));
	if ( (*hit == true) && ((flags & AKGL_COLLISION_TEST_PLANAR) != 0) ) {
	    collision_flatten(a, b, dest);
	}
	SUCCEED_RETURN(errctx);
    }

    collision_to_ccd(a, &obja);
    collision_to_ccd(b, &objb);
    collision_configure(&ccd);

    // One query, one arena. Resetting on entry rather than on exit means a query
    // that returns early still leaves it clean for the next one.
    akgl_ccd_arena_reset();

    /*
     * MPR and not GJK+EPA. MPR allocates nothing at all, converges in fewer
     * iterations, and its weakness -- a coarser contact *point* -- is on a field
     * the blocking resolver never reads. EPA stays compiled and available for a
     * caller who one day wants an accurate manifold and will pay the arena for
     * it.
     */
    result = ccdMPRPenetration(&obja, &objb, &ccd, &depth, &dir, &pos);
    if ( result == -2 ) {
	FAIL_RETURN(
	    errctx,
	    AKGL_ERR_COLLISION,
	    "Collision arena exhausted at %zu of %d bytes; raise AKGL_CCD_ARENA_BYTES",
	    akgl_ccd_arena_highwater(),
	    AKGL_CCD_ARENA_BYTES
	    );
    }
    if ( result != 0 ) {
	SUCCEED_RETURN(errctx);
    }

    /*
     * libccd hands back the direction that separates the *second* object. This
     * contact points at the first, so that a caller moving `a` along the normal
     * by the depth undoes the overlap -- which is what every resolver wants, and
     * saves each of them from remembering the sign.
     */
    dest->nx = -(float32_t)ccdVec3X(&dir);
    dest->ny = -(float32_t)ccdVec3Y(&dir);
    dest->nz = -(float32_t)ccdVec3Z(&dir);
    dest->depth = (float32_t)depth;
    dest->px = (float32_t)ccdVec3X(&pos);
    dest->py = (float32_t)ccdVec3Y(&pos);
    dest->pz = (float32_t)ccdVec3Z(&pos);

    if ( (flags & AKGL_COLLISION_TEST_PLANAR) != 0 ) {
	collision_flatten(a, b, dest);
    }
    *hit = true;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_arena_selftest(float32_t separation, bool *hit, float32_t *depth)
{
    collision_ccdobj a;
    collision_ccdobj b;
    ccd_t ccd;
    ccd_real_t ccddepth = 0.0;
    ccd_vec3_t dir;
    ccd_vec3_t pos;
    int result = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, hit, AKERR_NULLPOINTER, "NULL hit flag reference");
    FAIL_ZERO_RETURN(errctx, depth, AKERR_NULLPOINTER, "NULL depth reference");

    collision_configure(&ccd);

    a.kind = AKGL_COLLISION_SHAPE_BOX;
    ccdVec3Set(&a.pos, 0.0, 0.0, 0.0);
    ccdVec3Set(&a.half, 1.0, 1.0, 1.0);
    b.kind = AKGL_COLLISION_SHAPE_BOX;
    ccdVec3Set(&b.pos, (ccd_real_t)separation, 0.0, 0.0);
    ccdVec3Set(&b.half, 1.0, 1.0, 1.0);

    akgl_ccd_arena_reset();

    /*
     * ccdGJKPenetration and not ccdMPRPenetration, deliberately. This is the EPA
     * path, and EPA is the half of libccd that allocates -- so it is the half
     * that exercises the arena. The narrowphase proper uses MPR, which does not.
     */
    result = ccdGJKPenetration(&a, &b, &ccd, &ccddepth, &dir, &pos);

    if ( result == -2 ) {
	FAIL_RETURN(
	    errctx,
	    AKGL_ERR_COLLISION,
	    "Collision arena exhausted at %zu of %d bytes; raise AKGL_CCD_ARENA_BYTES",
	    akgl_ccd_arena_highwater(),
	    AKGL_CCD_ARENA_BYTES
	    );
    }

    *hit = (result == 0);
    *depth = (float32_t)ccddepth;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_partitioner_factory(akgl_Partitioner *self, char *type)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL partitioner reference");

    // NULL means "the default", which is the grid. Matched on leading
    // characters, the same way akgl_physics_factory matches its backends.
    if ( (type == NULL) || (strncmp(type, "grid", 4) == 0) ) {
	PASS(errctx, akgl_partitioner_init_grid(self));
	SUCCEED_RETURN(errctx);
    }
    if ( strncmp(type, "bsp", 3) == 0 ) {
	PASS(errctx, akgl_partitioner_init_bsp(self));
	SUCCEED_RETURN(errctx);
    }
    FAIL_RETURN(errctx, AKERR_KEY, "No partitioner named \"%s\"", type);
}

akerr_ErrorContext *akgl_collision_world_init(akgl_CollisionWorld *self, char *type, float32_t cellwidth, float32_t cellheight)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_NONZERO_RETURN(errctx, (cellwidth <= 0.0f), AKERR_VALUE,
			"Cell width %f is not positive", cellwidth);
    FAIL_NONZERO_RETURN(errctx, (cellheight <= 0.0f), AKERR_VALUE,
			"Cell height %f is not positive", cellheight);

    memset(self, 0x00, sizeof(akgl_CollisionWorld));
    self->cellwidth = cellwidth;
    self->cellheight = cellheight;
    // Every game today is 2D, so the guard is on by default. A caller with a
    // real third axis to resolve on clears it.
    self->flags = AKGL_COLLISION_TEST_PLANAR;
    self->tilelayermask = AKGL_COLLISION_LAYER_STATIC;

    PASS(errctx, akgl_partitioner_factory(&self->partitioner, type));
    PASS(errctx, self->partitioner.reset(&self->partitioner, self));
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Is a tile coordinate solid on any of the world's collidable layers?
 *
 * Reads the tilemap's own array. A gid of 0 is an empty cell; anything else is
 * a tile, and whether a tile is solid is decided by which layer it is drawn on,
 * not by the tile itself. Off the map is not solid -- see akgl_collision_solid_at.
 */
static bool collision_tile_solid(akgl_CollisionWorld *self, int32_t tx, int32_t ty)
{
    akgl_Tilemap *map = self->tilesource;
    int layer = 0;

    if ( (map == NULL) || (self->tilelayers == 0) ) {
	return false;
    }
    if ( (tx < 0) || (ty < 0) || (tx >= map->width) || (ty >= map->height) ) {
	return false;
    }

    for ( layer = 0; layer < map->numlayers; layer++ ) {
	if ( (self->tilelayers & (1u << layer)) == 0 ) {
	    continue;
	}
	// Row-major, strided by the map's width rather than the array's, which
	// is how akgl_tilemap_draw indexes it too.
	if ( map->layers[layer].data[(ty * map->width) + tx] != 0 ) {
	    return true;
	}
    }
    return false;
}

akerr_ErrorContext *akgl_collision_bind_tilemap(akgl_CollisionWorld *self, akgl_Tilemap *map)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");

    if ( map == NULL ) {
	self->tilesource = NULL;
	self->tilelayers = 0;
	SUCCEED_RETURN(errctx);
    }

    FAIL_NONZERO_RETURN(errctx, (map->tilewidth <= 0), AKERR_VALUE,
			"Map tile width %d is not positive", map->tilewidth);
    FAIL_NONZERO_RETURN(errctx, (map->tileheight <= 0), AKERR_VALUE,
			"Map tile height %d is not positive", map->tileheight);

    self->tilesource = map;
    self->tilelayers = map->collidablelayers;

    /*
     * Cells are keyed on the map's tiles. That is the sizing the performance
     * record argues for, and it makes the tile scan below exactly one cell's
     * worth of array reads per cell a query touches.
     */
    self->cellwidth = (float32_t)map->tilewidth;
    self->cellheight = (float32_t)map->tileheight;
    PASS(errctx, self->partitioner.reset(&self->partitioner, self));
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_solid_at(akgl_CollisionWorld *self, float32_t x, float32_t y, bool *dest)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "NULL result reference");

    *dest = false;
    if ( self->tilesource == NULL ) {
	SUCCEED_RETURN(errctx);
    }
    // floorf, not a cast: truncation rounds toward zero, so -0.5 and +0.5 would
    // land in the same tile and a coordinate just off the left edge of the map
    // would read as being on it.
    *dest = collision_tile_solid(self,
				 (int32_t)floorf(x / (float32_t)self->tilesource->tilewidth),
				 (int32_t)floorf(y / (float32_t)self->tilesource->tileheight));
    SUCCEED_RETURN(errctx);
}

/** @brief Carries the answer out of a query visitor. */
typedef struct {
    SDL_FRect box;
    bool blocked;
} collision_blocked_probe;

static akerr_ErrorContext *collision_blocked_visit(akgl_CollisionProxy *proxy, void *data)
{
    collision_blocked_probe *probe = (collision_blocked_probe *)data;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, proxy, AKERR_NULLPOINTER, "NULL proxy in a blocked probe");
    FAIL_ZERO_RETURN(errctx, probe, AKERR_NULLPOINTER, "NULL probe reference");

    if ( (proxy->shape.flags & AKGL_COLLISION_FLAG_SENSOR) != 0 ) {
	// A sensor reports, it does not block. Walking through a coin is not
	// being blocked by it.
	SUCCEED_RETURN(errctx);
    }
    if ( SDL_HasRectIntersectionFloat(&proxy->bounds, &probe->box) ) {
	probe->blocked = true;
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_box_blocked(akgl_CollisionWorld *self, SDL_FRect *box, uint32_t mask, bool *dest)
{
    collision_blocked_probe probe;
    int32_t tx = 0;
    int32_t ty = 0;
    int32_t x0 = 0;
    int32_t y0 = 0;
    int32_t x1 = 0;
    int32_t y1 = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_ZERO_RETURN(errctx, box, AKERR_NULLPOINTER, "NULL box reference");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "NULL result reference");

    *dest = false;

    if ( ((mask & self->tilelayermask) != 0) && (self->tilesource != NULL) ) {
	/*
	 * The far edge is nudged inward. Without it a box whose right edge sits
	 * exactly on a tile boundary reads as overlapping the tile beyond it,
	 * and an actor standing flush against a wall is reported as inside it --
	 * which makes every move it tries look blocked.
	 */
	x0 = (int32_t)floorf(box->x / (float32_t)self->tilesource->tilewidth);
	y0 = (int32_t)floorf(box->y / (float32_t)self->tilesource->tileheight);
	x1 = (int32_t)floorf(((box->x + box->w) - AKGL_COLLISION_TILE_EPSILON) / (float32_t)self->tilesource->tilewidth);
	y1 = (int32_t)floorf(((box->y + box->h) - AKGL_COLLISION_TILE_EPSILON) / (float32_t)self->tilesource->tileheight);

	for ( ty = y0; ty <= y1; ty++ ) {
	    for ( tx = x0; tx <= x1; tx++ ) {
		if ( collision_tile_solid(self, tx, ty) ) {
		    *dest = true;
		    SUCCEED_RETURN(errctx);
		}
	    }
	}
    }

    memset(&probe, 0x00, sizeof(probe));
    probe.box = *box;
    PASS(errctx, self->partitioner.query(&self->partitioner, box, mask, &collision_blocked_visit, &probe));
    *dest = probe.blocked;
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_query_box(akgl_CollisionWorld *self, SDL_FRect *box, uint32_t mask, akgl_CollisionVisitFunc visit, void *data)
{
    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_ZERO_RETURN(errctx, box, AKERR_NULLPOINTER, "NULL box reference");
    FAIL_ZERO_RETURN(errctx, visit, AKERR_NULLPOINTER, "NULL visitor reference");

    PASS(errctx, self->partitioner.query(&self->partitioner, box, mask, visit, data));
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_settle(akgl_CollisionWorld *self, akgl_CollisionShape *shape, float32_t *x, float32_t *y, int maxsteps)
{
    SDL_FRect box;
    bool blocked = false;
    float32_t step = 0.0f;
    int i = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_ZERO_RETURN(errctx, shape, AKERR_NULLPOINTER, "NULL shape reference");
    FAIL_ZERO_RETURN(errctx, x, AKERR_NULLPOINTER, "NULL x reference");
    FAIL_ZERO_RETURN(errctx, y, AKERR_NULLPOINTER, "NULL y reference");

    if ( maxsteps <= 0 ) {
	maxsteps = AKGL_COLLISION_SETTLE_STEPS;
    }
    step = self->cellheight;
    if ( step <= 0.0f ) {
	step = 1.0f;
    }

    for ( i = 0; i <= maxsteps; i++ ) {
	PASS(errctx, akgl_collision_shape_bounds(shape, *x, *y, &box));
	PASS(errctx, akgl_collision_box_blocked(self, &box, AKGL_COLLISION_LAYER_STATIC, &blocked));
	if ( blocked == false ) {
	    SUCCEED_RETURN(errctx);
	}
	// Up, because in a game with gravity the free space is above and the
	// floor is below. Lifting is the move that does not drop a character
	// through the world.
	*y -= step;
    }

    FAIL_RETURN(
	errctx,
	AKERR_VALUE,
	"A shape at %f, %f is still inside solid geometry after being lifted %d tiles; "
	"the object is probably placed wrong in the level rather than the limit being low",
	*x,
	*y,
	maxsteps
	);
}

/** @brief Carries one actor's resolution through the broad-phase visitor. */
typedef struct {
    akgl_CollisionWorld *world;
    akgl_Actor *actor;
    akgl_CollisionProxy *proxy;
    float32_t dt;
    akerr_ErrorContext *failure;
} collision_resolve_state;

/**
 * @brief Test one candidate against the actor being resolved, and answer it.
 *
 * The visitor cannot use CATCH: a `break` here would leave the broad phase's own
 * walk rather than this function, so a failure would look like the end of the
 * list. The status is carried out on the state instead and handed over by the
 * caller.
 */
static akerr_ErrorContext *collision_resolve_visit(akgl_CollisionProxy *other, void *data)
{
    collision_resolve_state *state = (collision_resolve_state *)data;
    akgl_Contact contact;
    bool interacts = false;
    bool hit = false;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, other, AKERR_NULLPOINTER, "NULL proxy in a resolve");
    FAIL_ZERO_RETURN(errctx, state, AKERR_NULLPOINTER, "NULL resolve state");

    if ( other == state->proxy ) {
	SUCCEED_RETURN(errctx);
    }

    /*
     * Nested rather than written with early returns. A *_RETURN inside an
     * ATTEMPT block leaves past CLEANUP, which is a real defect even where the
     * CLEANUP happens to be empty today -- and scripts/check_error_protocol.py
     * refuses it, correctly.
     */
    ATTEMPT {
	CATCH(errctx, akgl_collision_shape_interacts(&state->proxy->shape, &other->shape, &interacts));
	if ( interacts == true ) {
	    state->world->tests += 1;
	    CATCH(errctx, akgl_collision_test(state->proxy, other, state->world->flags, &contact, &hit));
	    if ( hit == true ) {
		contact.self = state->actor;
		contact.other = other->owner;
		contact.dt = state->dt;
		contact.sensor = (((state->proxy->shape.flags & AKGL_COLLISION_FLAG_SENSOR) != 0) ||
				  ((other->shape.flags & AKGL_COLLISION_FLAG_SENSOR) != 0));
		contact.statichit = ((other->shape.flags & AKGL_COLLISION_FLAG_STATIC) != 0);

		CATCH(errctx, state->actor->collidefunc(state->actor, &contact));

		// The actor moved, so its own proxy is stale for the rest of the pass.
		CATCH(errctx, akgl_collision_proxy_sync(state->proxy, &state->actor->shape,
							state->actor->x, state->actor->y,
							state->actor->z));
	    }
	}
    } CLEANUP {
    } PROCESS(errctx) {
    } FINISH(errctx, true);
    SUCCEED_RETURN(errctx);
}

/**
 * @brief Resolve one actor against the tiles its shape overlaps.
 *
 * Tiles are not proxies, so they are scanned rather than queried: a stack-local
 * proxy is synthesized for each solid cell and handed to the ordinary
 * narrowphase, which keeps one code path for "what does a contact look like"
 * rather than two.
 */
static akerr_ErrorContext *collision_resolve_tiles(akgl_CollisionWorld *self, akgl_Actor *actor,
						   akgl_CollisionProxy *proxy, float32_t dt)
{
    akgl_CollisionProxy tile;
    akgl_CollisionShape tileshape;
    akgl_Contact contact;
    SDL_FRect body;
    bool hit = false;
    int32_t tx = 0;
    int32_t ty = 0;
    int32_t x0 = 0;
    int32_t y0 = 0;
    int32_t x1 = 0;
    int32_t y1 = 0;
    int layer = 0;

    PREPARE_ERROR(errctx);

    if ( (self->tilesource == NULL) || (self->tilelayers == 0) ) {
	SUCCEED_RETURN(errctx);
    }
    if ( (proxy->shape.collidemask & self->tilelayermask) == 0 ) {
	SUCCEED_RETURN(errctx);
    }

    x0 = (int32_t)floorf(proxy->bounds.x / (float32_t)self->tilesource->tilewidth);
    y0 = (int32_t)floorf(proxy->bounds.y / (float32_t)self->tilesource->tileheight);
    x1 = (int32_t)floorf(((proxy->bounds.x + proxy->bounds.w) - AKGL_COLLISION_TILE_EPSILON) /
			 (float32_t)self->tilesource->tilewidth);
    y1 = (int32_t)floorf(((proxy->bounds.y + proxy->bounds.h) - AKGL_COLLISION_TILE_EPSILON) /
			 (float32_t)self->tilesource->tileheight);

    body.w = (float32_t)self->tilesource->tilewidth;
    body.h = (float32_t)self->tilesource->tileheight;
    body.x = 0.0f;
    body.y = 0.0f;

    for ( ty = y0; ty <= y1; ty++ ) {
	for ( tx = x0; tx <= x1; tx++ ) {
	    if ( !collision_tile_solid(self, tx, ty) ) {
		continue;
	    }

	    ATTEMPT {
		CATCH(errctx, akgl_collision_shape_box(&tileshape, &body, 0.0f));
		tileshape.layermask = self->tilelayermask;
		tileshape.flags |= AKGL_COLLISION_FLAG_STATIC;
		CATCH(errctx, akgl_collision_proxy_initialize(
			  &tile, NULL, &tileshape,
			  (float32_t)(tx * self->tilesource->tilewidth),
			  (float32_t)(ty * self->tilesource->tileheight), proxy->z));

		CATCH(errctx, akgl_collision_test(proxy, &tile, self->flags, &contact, &hit));
		self->tests += 1;
		if ( hit == true ) {
		    contact.self = actor;
		    contact.other = NULL;
		    contact.dt = dt;
		    contact.statichit = true;
		    contact.tilex = tx;
		    contact.tiley = ty;
		    contact.tilegid = 0;
		    contact.tilelayer = -1;
		    // Which layer and which tile, so a game can tell a spike from
		    // a floor without looking it up again.
		    for ( layer = 0; layer < self->tilesource->numlayers; layer++ ) {
			if ( (self->tilelayers & (1u << layer)) == 0 ) {
			    continue;
			}
			if ( self->tilesource->layers[layer].data[(ty * self->tilesource->width) + tx] != 0 ) {
			    contact.tilelayer = layer;
			    contact.tilegid = self->tilesource->layers[layer].data[(ty * self->tilesource->width) + tx];
			    break;
			}
		    }

		    CATCH(errctx, actor->collidefunc(actor, &contact));
		    CATCH(errctx, akgl_collision_proxy_sync(proxy, &actor->shape,
							    actor->x, actor->y, actor->z));
		}
	    } CLEANUP {
	    } PROCESS(errctx) {
	    } FINISH(errctx, true);
	}
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_resolve(akgl_CollisionWorld *self, akgl_Actor *actor, float32_t dt)
{
    collision_resolve_state state;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "NULL actor reference");

    if ( (actor->proxy == NULL) || (actor->shape.kind == AKGL_COLLISION_SHAPE_NONE) ) {
	SUCCEED_RETURN(errctx);
    }
    if ( actor->collidefunc == NULL ) {
	SUCCEED_RETURN(errctx);
    }

    // The actor has just moved, so the index and the copy of its shape are both
    // a step out of date.
    PASS(errctx, akgl_collision_proxy_sync(actor->proxy, &actor->shape, actor->x, actor->y, actor->z));
    PASS(errctx, self->partitioner.move(&self->partitioner, actor->proxy));

    PASS(errctx, collision_resolve_tiles(self, actor, actor->proxy, dt));

    memset(&state, 0x00, sizeof(state));
    state.world = self;
    state.actor = actor;
    state.proxy = actor->proxy;
    state.dt = dt;
    PASS(errctx, self->partitioner.query(&self->partitioner, &actor->proxy->bounds,
					 actor->shape.collidemask, &collision_resolve_visit, &state));

    // Whatever the responses did, the index has to end the pass agreeing with
    // where the actor actually is.
    PASS(errctx, akgl_collision_proxy_sync(actor->proxy, &actor->shape, actor->x, actor->y, actor->z));
    PASS(errctx, self->partitioner.move(&self->partitioner, actor->proxy));
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_substeps(akgl_CollisionWorld *self, akgl_Actor *actor, float32_t dt, int *dest)
{
    float32_t span = 0.0f;
    float32_t limit = 0.0f;
    float32_t thin = 0.0f;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, actor, AKERR_NULLPOINTER, "NULL actor reference");
    FAIL_ZERO_RETURN(errctx, dest, AKERR_NULLPOINTER, "NULL destination reference");

    /*
     * One, unless there is something to collide with. A step of exactly `dt`
     * takes the same arithmetic path it always has -- `dt / 1.0f` is `dt`
     * exactly in IEEE-754 -- which is what lets every existing assertion about
     * a non-colliding actor hold unchanged.
     */
    *dest = 1;
    if ( (self == NULL) || (actor->shape.kind == AKGL_COLLISION_SHAPE_NONE) ) {
	SUCCEED_RETURN(errctx);
    }

    span = fabsf(actor->vx);
    if ( fabsf(actor->vy) > span ) { span = fabsf(actor->vy); }
    if ( fabsf(actor->vz) > span ) { span = fabsf(actor->vz); }
    span = span * dt;

    /*
     * Do not travel further in one sub-step than the thinnest thing that could
     * be passed through. That is a cell *or the actor's own extent*, whichever
     * is smaller: a small fast actor is the one that tunnels, and bounding only
     * by cell size misses it entirely.
     */
    thin = self->cellwidth;
    if ( self->cellheight < thin ) { thin = self->cellheight; }
    if ( (2.0f * actor->shape.hx) < thin ) { thin = 2.0f * actor->shape.hx; }
    if ( (2.0f * actor->shape.hy) < thin ) { thin = 2.0f * actor->shape.hy; }
    limit = AKGL_COLLISION_SUBSTEP_FRACTION * thin;

    if ( limit <= 0.0f ) {
	SUCCEED_RETURN(errctx);
    }
    *dest = (int)(span / limit) + 1;
    if ( *dest > AKGL_COLLISION_MAX_SUBSTEPS ) {
	*dest = AKGL_COLLISION_MAX_SUBSTEPS;
    }
    SUCCEED_RETURN(errctx);
}

akerr_ErrorContext *akgl_collision_sync_actors(akgl_CollisionWorld *self)
{
    akgl_Actor *actor = NULL;
    int i = 0;

    PREPARE_ERROR(errctx);
    FAIL_ZERO_RETURN(errctx, self, AKERR_NULLPOINTER, "NULL collision world reference");

    /*
     * One pass over the pool: give a proxy to anything that has gained a shape,
     * take one back from anything that has lost one, and refresh the rest. This
     * is the only place a proxy is created or destroyed during a step, so
     * nothing below has to think about lifetimes.
     */
    for ( i = 0; i < AKGL_MAX_HEAP_ACTOR; i++ ) {
	actor = &akgl_heap_actors[i];
	if ( actor->refcount == 0 ) {
	    continue;
	}

	if ( actor->shape.kind == AKGL_COLLISION_SHAPE_NONE ) {
	    if ( actor->proxy != NULL ) {
		PASS(errctx, self->partitioner.remove(&self->partitioner, actor->proxy));
		PASS(errctx, akgl_heap_release_collision_proxy(actor->proxy));
		actor->proxy = NULL;
	    }
	    continue;
	}

	if ( actor->proxy == NULL ) {
	    PASS(errctx, akgl_heap_next_collision_proxy(&actor->proxy));
	    PASS(errctx, akgl_collision_proxy_initialize(actor->proxy, actor, &actor->shape,
							 actor->x, actor->y, actor->z));
	    PASS(errctx, self->partitioner.insert(&self->partitioner, actor->proxy));
	    continue;
	}

	PASS(errctx, akgl_collision_proxy_sync(actor->proxy, &actor->shape,
					       actor->x, actor->y, actor->z));
	PASS(errctx, self->partitioner.move(&self->partitioner, actor->proxy));
    }
    SUCCEED_RETURN(errctx);
}