collision grid

/*
    Collision grid, 16x16 tiles

    - create
    + set( x16, y16 )
    + unset( x16, y16 )
    + test( x, y )
    + raycast( x1, y1, x2, y2, notestfirst ) => null / dt, isp_x, isp_y
    + aabbtest( x1, y1, x2, y2 ) => null / true, xnrm, ynrm, push
*/

global ColGrid = {};

function ColGrid.create()
{
    data =
    {
        tiles = map(),
    };
    return class( data, ColGrid );
}

function ColGrid._getkey( x16, y16 )
{
    return ((x16&0xffff)<<16) | (y16&0xffff);
}

function ColGrid.set( x16, y16 )
{
    this.tiles[ this._getkey( x16, y16 ) ] = true;
}

function ColGrid.unset( x16, y16 )
{
    unset( this.tiles, this._getkey( x16, y16 ) );
}

function ColGrid.test( x, y )
{
    x16 = toint( floor( x / 16 ) );
    y16 = toint( floor( y / 16 ) );
    return isset( this.tiles, this._getkey( x16, y16 ) );
}

function ColGrid.raycast( x1, y1, x2, y2, notestfirst )
{
    x1_16 = toint( floor( x1 / 16 ) );
    y1_16 = toint( floor( y1 / 16 ) );
    x2_16 = toint( floor( x2 / 16 ) );
    y2_16 = toint( floor( y2 / 16 ) );

    if( !notestfirst && isset( this.tiles, this._getkey( x1_16, y1_16 ) ) )
        return 0, x1, y1;
    if( x1_16 == x2_16 && y1_16 == y2_16 )
        return null;

    x16 = x1_16;
    y16 = y1_16;
    xdist = x2 - x1;
    ydist = y2 - y1;
    stepX = if( xdist >= 0, 1, -1 );
    stepY = if( ydist >= 0, 1, -1 );
    len = sqrt( xdist * xdist + ydist * ydist );
    if( xdist == 0 )
    {
        tMaxX = 3.8e+10;
        tDeltaX = 0;
    }
    else
    {
        tMaxX = ( floor( x1_16 + ( xdist > 0 ) ) * 16 - x1 ) * len / xdist; // should be +
        tDeltaX = 16 * len / abs( xdist );
    }
    if( ydist == 0 )
    {
        tMaxY = 3.8e+10;
        tDeltaY = 0;
    }
    else
    {
        tMaxY = ( floor( y1_16 + ( ydist > 0 ) ) * 16 - y1 ) * len / ydist; // should be +
        tDeltaY = 16 * len / abs( ydist );
    }
    t = if( tMaxX < tMaxY, tMaxX, tMaxY );

    while( x16 != x2_16 || y16 != y2_16 )
    {
        if( tMaxX < tMaxY )
        {
            t += tMaxX;
            tMaxX += tDeltaX;
            x16 += stepX;
        }
        else
        {
            t = tMaxY;
            tMaxY += tDeltaY;
            y16 += stepY;
        }

        if( isset( this.tiles, this._getkey( x16, y16 ) ) )
        {
            q = t / len;
            q1 = 1 - q;
            return q, x1 * q1 + x2 * q, y1 * q1 + y2 * q;
        }
    }

    return null;
}

function _aabb_aabb_resolve( ax1, ay1, ax2, ay2, bx1, by1, bx2, by2 )
{
    // in an intersection all of these are non-negative
    dx1 = ax2 - bx1;
    dx2 = bx2 - ax1;
    dy1 = ay2 - by1;
    dy2 = by2 - ay1;

    // find smallest of those and resolve assuming BB1 is to be moved
    if( dx1 < dx2 )
    {
        if( dx1 < dy1 )
        {
            if( dx1 < dy2 )
                return -dx1, 0;
            else // dy2
                return 0, dy2;
        }
        else // dy1
        {
            if( dy1 < dy2 )
                return 0, -dy1;
            else // dy2
                return 0, dy2;
        }
    }
    else // dx2
    {
        if( dx2 < dy1 )
        {
            if( dx2 < dy2 )
                return dx2, 0;
            else
                return 0, dy2;
        }
        else // dy1
        {
            if( dy1 < dy2 )
                return 0, -dy1;
            else // dy2
                return 0, dy2;
        }
    }
}

function ColGrid.aabbtest( x1, y1, x2, y2 )
{
    x1_16 = toint( floor( x1 / 16 ) );
    y1_16 = toint( floor( y1 / 16 ) );
    x2_16 = toint( floor( x2 / 16 ) );
    y2_16 = toint( floor( y2 / 16 ) );

    nx = 0;
    ny = 0;
    isc = 0;

    for( y16 = y1_16; y16 <= y2_16; ++y16 )
    {
        for( x16 = x1_16; x16 <= x2_16; ++x16 )
        {
            if( isset( this.tiles, this._getkey( x16, y16 ) ) )
            {
                (cx,cy) = _aabb_aabb_resolve( x1, y1, x2, y2, x16 * 16, y16 * 16, x16 * 16 + 16, y16 * 16 + 16 );
                nx += cx;
                ny += cy;
                isc++;
            }
        }
    }

    if( isc )
    {
        nx /= isc;
        ny /= isc;
    }

    if( nx != 0 || ny != 0 )
    {
        nd = sqrt( nx * nx + ny * ny );
        nx /= nd;
        ny /= nd;
        return true, nx, ny, nd;
    }
}