Skip to content

Instantly share code, notes, and snippets.

@AndreiRudenko

AndreiRudenko/SpatialHash.hx

Last active May 13, 2019 19:01
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save AndreiRudenko/2bc27bb7af9338699cda to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save AndreiRudenko/2bc27bb7af9338699cda to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. AndreiRudenko revised this gist Jul 24, 2016. 1 changed file with 128 additions and 166 deletions.
    294 changes: 128 additions & 166 deletions SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -1,225 +1,187 @@
    // SpatialHash uniform-grid implementation
    // Broad-phase algorithm for collision detection
    // Andrei Rudenko // SpatialHash.hx (10.08.2015)
    // Andrei Rudenko // SpatialHash.hx (24.07.2016)

    import luxe.Vector;
    import luxe.utils.Maths;

    class SpatialHash {

    public var min(default, null):Vector;
    public var max(default, null):Vector;

    public var pos:Vector;
    /* the square cell gridLength of the grid. Must be larger than the largest shape in the space. */
    public var cellSize(default, null):Int;
    public var cellSize(default, set):UInt;
    /* the world space width */
    public var gridWidth (default, null):Int;
    public var w (default, null):Int;
    /* the world space height */
    public var gridHeight (default, null):Int;
    public var h (default, null):Int;
    /* the number of buckets (i.e. cells) in the spatial grid */
    public var gridLength (default, null):Int;
    /* the array-list holding the spatial grid buckets */
    public var grid (default, null):haxe.ds.Vector<Array<Body>>;

    var invCellSize:Float;
    var gridWidthRes:Float;
    var gridHeightRes:Float;
    public var grid(default, null) : haxe.ds.Vector<Array<Collider>>;

    // for example: new SpatialHash(1280, 720, 32);
    public function new( _width:Float, _height:Float, _cellSize:Int) {
    public var width(default, null):Float;
    public var height(default, null):Float;

    gridWidthRes = _width;
    gridHeightRes = _height;
    var powerOfTwo:UInt;

    cellSize = _cellSize;
    invCellSize = 1/cellSize;
    // temp
    var _tmp_getGridIndexesArray:Array<Int>;

    gridWidth = Math.ceil(_width * invCellSize);
    gridHeight = Math.ceil(_height * invCellSize);
    public function new( _min:Vector, _max:Vector, _cs:UInt) {

    gridLength = Std.int(gridWidth * gridHeight);
    min = _min.clone();
    max = _max.clone();

    grid = new haxe.ds.Vector(gridLength);
    pos = new Vector();

    for (i in 0...gridLength) {
    grid[i] = new Array<Body>();
    }
    }
    width = max.x - min.x;
    height = max.y - min.y;

    public function addBody(b:Body){
    addIndex( b, getIndex1DVec(clampToGridVec(b.pos.clone())) );
    }
    cellSize = _cs;

    public function removeBody(b:Body):Void{
    removeIndexes(b);
    }
    w = Math.ceil(width) >> powerOfTwo;
    h = Math.ceil(height) >> powerOfTwo;

    public function updateBody(b:Body){
    updateIndexes(b, aabbToGrid(b.aabb.min, b.aabb.max));
    getBodyCollision(b);
    }

    inline function getIndex1DVec(_pos:Vector):Int { // i = x + w * y; x = i % w; y = i / w;
    return Std.int(Math.floor(_pos.x * invCellSize) + gridWidth * Math.floor(_pos.y * invCellSize) );
    }

    inline function getIndex(_pos:Float):Int {
    return Std.int(_pos * invCellSize);
    }
    gridLength = Std.int(w * h);

    inline function getIndex1D(_x:Int, _y:Int):Int { // i = x + w * y; x = i % w; y = i / w;
    return Std.int(_x + gridWidth * _y);
    }
    grid = new haxe.ds.Vector(gridLength);

    function updateIndexes(b:Body, _ar:Array<Int>) {
    for (i in b.gridIndex) {
    removeIndex(b, i);
    for (i in 0...gridLength) {
    grid[i] = new Array<Collider>();
    }

    b.gridIndex.splice(0, b.gridIndex.length);
    // temp
    _tmp_getGridIndexesArray = [];

    for (i in _ar) {
    addIndex(b, i);
    }
    }

    function addIndex(b:Body, _cellPos:Int){
    grid[_cellPos].push(b);
    b.gridIndex.push(_cellPos);
    }

    function removeIndexes(b:CellObject){
    for (i in b.gridIndex) {
    removeIndex(b, i);
    }
    public function addCollider(c:Collider){
    updateIndexes(c, aabbToGrid(c.aabb.min, c.aabb.max ));
    }

    b.gridIndex.splice(0, b.gridIndex.length);
    public function removeCollider(c:Collider):Void{
    removeIndexes(c);
    }

    function removeIndex(b:CellObject, _pos:Int) {
    grid[_pos].remove(b);
    // b.gridIndex.remove(_pos);
    public function updateCollider(c:Collider){
    updateIndexes(c, aabbToGrid(c.aabb.min, c.aabb.max));
    findContacts(c);
    }

    function isValidGridPos(num:Int):Bool {
    if(num < 0 || num >= gridLength){
    return false;
    } else {
    return true;
    public function empty(){
    for (cell in grid) {
    if(cell.length > 0){
    for (c in cell) {
    c.gridIndex.splice(0, c.gridIndex.length);
    }
    cell.splice(0, cell.length);
    }
    }
    }

    function clampToGridVec(_vec:Vector):Vector {
    _vec.x = Maths.clamp(_vec.x, 0, gridWidthRes-1);
    _vec.y = Maths.clamp(_vec.y, 0, gridHeightRes-1);
    return _vec;
    public function destroy(){
    empty();
    min = null;
    max = null;
    pos = null;
    grid = null;
    _tmp_getGridIndexesArray = null;
    }

    function getBodyCollision(_bd:Body) {
    for (i in _bd.gridIndex) {
    if(grid[i].length == 0) continue;
    for (cbd in grid[i]) {
    // add contacts here
    // addCollisionPair(_bd, cbd);
    inline public function findContacts(collider:Collider) {
    for (i in collider.gridIndex) {
    for (otherCollider in grid[i]) {
    if(collider == otherCollider) continue;
    // add contacts here
    // addContact(collider, otherCollider);
    }
    }

    }

    function aabbToGrid(_min:Vector, _max:Vector):Array<Int> {
    var arr:Array<Int> = [];
    inline function aabbToGrid(_min:Vector, _max:Vector):Array<Int> {
    var ret:Array<Int> = _tmp_getGridIndexesArray;
    ret.splice(0, ret.length);

    var aabbMinX:Int = Maths.clampi(getIndex(_min.x), 0, gridWidth-1);
    var aabbMinY:Int = Maths.clampi(getIndex(_min.y), 0, gridHeight-1);
    var aabbMaxX:Int = Maths.clampi(getIndex(_max.x), 0, gridWidth-1);
    var aabbMaxY:Int = Maths.clampi(getIndex(_max.y), 0, gridHeight-1);
    if(!overlaps(_min, _max)) return ret;

    var aabbMinX:Int = Maths.clampi(getIndex_X(_min.x), 0, w-1);
    var aabbMinY:Int = Maths.clampi(getIndex_Y(_min.y), 0, h-1);
    var aabbMaxX:Int = Maths.clampi(getIndex_X(_max.x), 0, w-1);
    var aabbMaxY:Int = Maths.clampi(getIndex_Y(_max.y), 0, h-1);

    var aabbMin:Int = getIndex1D(aabbMinX, aabbMinY);
    var aabbMax:Int = getIndex1D(aabbMaxX, aabbMaxY);
    var aabbMin:Int = getIndex1d(aabbMinX, aabbMinY);
    var aabbMax:Int = getIndex1d(aabbMaxX, aabbMaxY);

    arr.push(aabbMin);
    ret.push(aabbMin);
    if(aabbMin != aabbMax) {
    arr.push(aabbMax);
    ret.push(aabbMax);

    var lenX:Int = aabbMaxX - aabbMinX + 1;
    var lenY:Int = aabbMaxY - aabbMinY + 1;
    for (x in 0...lenX) {
    for (y in 0...lenY) {
    // пропускаем добавленые
    if((x == 0 && y == 0) || (x == lenX-1 && y == lenY-1) ){
    continue;
    }
    arr.push(getIndex1D(x, y) + aabbMin);
    if((x == 0 && y == 0) || (x == lenX-1 && y == lenY-1) ) continue;
    ret.push(getIndex1d(x, y) + aabbMin);
    }
    }
    }

    return arr;
    }

    /* DDA line algorithm. @author playchilla.com */
    public function lineToGrid(x1:Float, y1:Float, x2:Float, y2:Float ):Array<Int>{
    var arr:Array<Int> = [];

    var gridPosX:Int = getIndex(x1);
    var gridPosY:Int = getIndex(y1);

    if(!isValidGridPos(gridPosX, gridWidth) || !isValidGridPos(gridPosY, gridHeight)){
    return arr;
    }

    arr.push(getIndex1D(gridPosX, gridPosY));

    var dirX:Float = x2 - x1;
    var dirY:Float = y2 - y1;
    var distSqr:Float = dirX * dirX + dirY * dirY;
    if (distSqr < Constants.EPSILON){
    return arr;
    }

    var nf:Float = 1 / Math.sqrt(distSqr);
    dirX *= nf;
    dirY *= nf;

    var deltaX:Float = cellSize / Math.abs(dirX);
    var deltaY:Float = cellSize / Math.abs(dirY);

    var maxX:Float = gridPosX * cellSize - x1;
    var maxY:Float = gridPosY * cellSize - y1;
    if (dirX >= 0) maxX += cellSize;
    if (dirY >= 0) maxY += cellSize;
    maxX /= dirX;
    maxY /= dirY;

    var stepX:Int = Maths.sign(dirX);
    var stepY:Int = Maths.sign(dirY);
    var gridGoalX:Int = getIndex(x2);
    var gridGoalY:Int = getIndex(y2);
    var currentDirX:Int = gridGoalX - gridPosX;
    var currentDirY:Int = gridGoalY - gridPosY;

    while (currentDirX * stepX > 0 || currentDirY * stepY > 0) {
    if (maxX < maxY) {
    maxX += deltaX;
    gridPosX += stepX;
    currentDirX = gridGoalX - gridPosX;
    } else {
    maxY += deltaY;
    gridPosY += stepY;
    currentDirY = gridGoalY - gridPosY;
    }

    if(!isValidGridPos(gridPosX, gridWidth) || !isValidGridPos(gridPosY, gridHeight)){
    break;
    }
    arr.push(getIndex1D(gridPosX, gridPosY));
    }

    return arr;
    }

    public function clear(){
    for (cell in grid) {
    if(cell.length > 0){
    for (co in cell) {
    co.gridIndex.splice(0, co.gridIndex.length);
    }
    cell.splice(0, cell.length);
    }
    }
    return ret;
    }

    function updateIndexes(c:Collider, _ar:Array<Int>) {
    for (i in c.gridIndex) {
    removeIndex(c, i);
    }

    c.gridIndex.splice(0, c.gridIndex.length);
    for (i in _ar) {
    addIndexes(c, i);
    }
    }

    function removeIndexes(c:Collider){
    for (i in c.gridIndex) {
    removeIndex(c, i);
    }
    c.gridIndex.splice(0, c.gridIndex.length);
    }

    inline function addIndexes(c:Collider, _cellPos:Int){
    grid[_cellPos].push(c);
    c.gridIndex.push(_cellPos);
    }

    inline function removeIndex(c:Collider, _pos:Int) {
    grid[_pos].remove(c);
    }

    inline function getIndex_X(_pos:Float):Int {
    return Std.int((_pos - (pos.x + min.x))) >> powerOfTwo;
    }

    inline function getIndex_Y(_pos:Float):Int {
    return Std.int((_pos - (pos.y + min.y))) >> powerOfTwo;
    }

    inline function getIndex1d(_x:Int, _y:Int):Int { // i = x + w * y; x = i % w; y = i / w;
    return Std.int(_x + w * _y);
    }

    inline function overlaps(_min:Vector, _max:Vector):Bool {
    if ( _max.x < (pos.x + min.x) || _min.x > (pos.x + max.x) ) return false;
    if ( _max.y < (pos.y + min.y) || _min.y > (pos.y + max.y) ) return false;
    return true;
    }

    function set_cellSize(value:UInt):UInt {
    powerOfTwo = Math.round(Math.log(value)/Math.log(2));
    cellSize = 1 << powerOfTwo;
    return cellSize;
    }
    }
  2. AndreiRudenko revised this gist Aug 10, 2015. 1 changed file with 0 additions and 1 deletion.
    1 change: 0 additions & 1 deletion SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -92,7 +92,6 @@ class SpatialHash {
    }

    function removeIndex(b:CellObject, _pos:Int) {
    resetCellDebug(_pos);
    grid[_pos].remove(b);
    // b.gridIndex.remove(_pos);
    }
  3. AndreiRudenko revised this gist Aug 10, 2015. 1 changed file with 129 additions and 92 deletions.
    221 changes: 129 additions & 92 deletions SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -1,6 +1,6 @@
    // SpatialHash uniform-grid implementation
    // Broad-phase algorithm for collision detection
    // Andrei Rudenko // SpatialHash.hx (28.07.2015)
    // Andrei Rudenko // SpatialHash.hx (10.08.2015)

    import luxe.Vector;

    @@ -42,147 +42,184 @@ class SpatialHash {
    }

    public function addBody(b:Body){
    addIndex( b, 0, getGridPos(clampToGridPos(b.pos.clone())) );
    addIndex( b, getIndex1DVec(clampToGridVec(b.pos.clone())) );
    }

    public function removeBody(b:Body):Void{
    removeIndexes(b);
    }

    public function updateBody(b:Body){
    var arr:Array<Int> = aabbToGrid(b.aabb.min, b.aabb.max);
    removeIndexes(b);
    updateBodyIndexArrayLen(b.gridIndex, arr);
    moveIndexes(b, arr);
    updateIndexes(b, aabbToGrid(b.aabb.min, b.aabb.max));
    getBodyCollision(b);
    }

    function getGridPos(_pos:Vector):Int { // i = x + w * y; x = i % w; y = i / w;
    inline function getIndex1DVec(_pos:Vector):Int { // i = x + w * y; x = i % w; y = i / w;
    return Std.int(Math.floor(_pos.x * invCellSize) + gridWidth * Math.floor(_pos.y * invCellSize) );
    }

    function getGridX(_ind:Int):Int { // x = i % w;
    return _ind % gridWidth;
    }

    function getGridY(_ind:Int):Int { // y = i / w;
    return Std.int(_ind / gridWidth);
    inline function getIndex(_pos:Float):Int {
    return Std.int(_pos * invCellSize);
    }

    function addIndex(_bd:Body, _bodyGridIndex:Int, _cellPos:Int){
    grid[_cellPos].push(_bd);
    _bd.gridIndex[_bodyGridIndex] = _cellPos;
    inline function getIndex1D(_x:Int, _y:Int):Int { // i = x + w * y; x = i % w; y = i / w;
    return Std.int(_x + gridWidth * _y);
    }

    function moveIndexes(_bd:Body, _ar:Array<Int>):Void{
    for (i in 0..._bd.gridIndex.length) {
    addIndex(_bd, i, _ar[i]);
    function updateIndexes(b:Body, _ar:Array<Int>) {
    for (i in b.gridIndex) {
    removeIndex(b, i);
    }
    }

    function removeIndexes(_bd:Body):Void{
    for (i in _bd.gridIndex) {
    removeIndex(_bd, i);
    b.gridIndex.splice(0, b.gridIndex.length);

    for (i in _ar) {
    addIndex(b, i);
    }
    }

    function removeIndex(_bd:Body, _pos:Int) {
    reset_cell_debug(_pos);
    grid[_pos].remove(_bd);
    function addIndex(b:Body, _cellPos:Int){
    grid[_cellPos].push(b);
    b.gridIndex.push(_cellPos);
    }

    function aabbToGrid(_min:Vector, _max:Vector):Array<Int> {
    function removeIndexes(b:CellObject){
    for (i in b.gridIndex) {
    removeIndex(b, i);
    }

    clampToGridPos(_min);
    clampToGridPos(_max);
    var aabb_min:Int = getGridPos(_min);
    var aabb_max:Int = getGridPos(_max);
    b.gridIndex.splice(0, b.gridIndex.length);
    }

    var arr:Array<Int> = [];
    arr.push(aabb_min);
    arr.push(aabb_max);
    function removeIndex(b:CellObject, _pos:Int) {
    resetCellDebug(_pos);
    grid[_pos].remove(b);
    // b.gridIndex.remove(_pos);
    }

    if(arr.length > 1){
    var lenX:Int = getGridX(arr[1]) - getGridX(arr[0]) + 1;
    var lenY:Int = getGridY(arr[1]) - getGridY(arr[0]) + 1;
    for (x in 0...lenX) {
    for (y in 0...lenY) {
    // пропускаем добавленые
    if((x == 0 && y == 0) || (x == lenX-1 && y == lenY-1) ){
    continue;
    }
    arr.push(x + gridWidth * y + arr[0]);
    }
    }
    function isValidGridPos(num:Int):Bool {
    if(num < 0 || num >= gridLength){
    return false;
    } else {
    return true;
    }
    return arr;
    }

    function clampToGridVec(_vec:Vector):Vector {
    _vec.x = Maths.clamp(_vec.x, 0, gridWidthRes-1);
    _vec.y = Maths.clamp(_vec.y, 0, gridHeightRes-1);
    return _vec;
    }

    function getBodyCollision(_bd:Body) {
    // добавление
    // проверяем все яйчейки
    for (i in _bd.gridIndex) {
    // проверяем есть ли ктото в этой яйчейке
    if(grid[i].length == 0) continue;
    // проверяем кто есть в ней
    for (cbd in grid[i]) {
    // add contacts here
    // addCollisionPair(_bd, cbd);
    }
    }
    }

    function isValidGridPos(num:Int):Bool {
    if(num < 0 || num >= gridLength){
    return false;
    } else {
    return true;
    }
    }
    function aabbToGrid(_min:Vector, _max:Vector):Array<Int> {
    var arr:Array<Int> = [];

    function clampToGrid(num:Int):Int {
    if( num < 0 ) {
    return 0;
    } else if(num >= gridLength) {
    return gridLength-1;
    } else {
    return num;
    }
    }
    var aabbMinX:Int = Maths.clampi(getIndex(_min.x), 0, gridWidth-1);
    var aabbMinY:Int = Maths.clampi(getIndex(_min.y), 0, gridHeight-1);
    var aabbMaxX:Int = Maths.clampi(getIndex(_max.x), 0, gridWidth-1);
    var aabbMaxY:Int = Maths.clampi(getIndex(_max.y), 0, gridHeight-1);

    function clampToGridPos(_vec:Vector):Vector {
    if(_vec.x < 0){
    _vec.x = 0;
    }
    if(_vec.y < 0){
    _vec.y = 0;
    }
    if(_vec.x >= gridWidthRes){
    _vec.x = gridWidthRes-1;
    }
    if(_vec.y >= gridHeightRes){
    _vec.y = gridHeightRes-1;
    var aabbMin:Int = getIndex1D(aabbMinX, aabbMinY);
    var aabbMax:Int = getIndex1D(aabbMaxX, aabbMaxY);

    arr.push(aabbMin);
    if(aabbMin != aabbMax) {
    arr.push(aabbMax);

    var lenX:Int = aabbMaxX - aabbMinX + 1;
    var lenY:Int = aabbMaxY - aabbMinY + 1;
    for (x in 0...lenX) {
    for (y in 0...lenY) {
    // пропускаем добавленые
    if((x == 0 && y == 0) || (x == lenX-1 && y == lenY-1) ){
    continue;
    }
    arr.push(getIndex1D(x, y) + aabbMin);
    }
    }
    }
    return _vec;

    return arr;
    }

    function updateBodyIndexArrayLen(gridAr:Array<Int>, ar:Array<Int>){
    while(gridAr.length != ar.length) {
    if(gridAr.length > ar.length){
    gridAr.pop();
    } else if (gridAr.length < ar.length){
    gridAr.push(0);
    }
    }
    /* DDA line algorithm. @author playchilla.com */
    public function lineToGrid(x1:Float, y1:Float, x2:Float, y2:Float ):Array<Int>{
    var arr:Array<Int> = [];

    var gridPosX:Int = getIndex(x1);
    var gridPosY:Int = getIndex(y1);

    if(!isValidGridPos(gridPosX, gridWidth) || !isValidGridPos(gridPosY, gridHeight)){
    return arr;
    }

    arr.push(getIndex1D(gridPosX, gridPosY));

    var dirX:Float = x2 - x1;
    var dirY:Float = y2 - y1;
    var distSqr:Float = dirX * dirX + dirY * dirY;
    if (distSqr < Constants.EPSILON){
    return arr;
    }

    var nf:Float = 1 / Math.sqrt(distSqr);
    dirX *= nf;
    dirY *= nf;

    var deltaX:Float = cellSize / Math.abs(dirX);
    var deltaY:Float = cellSize / Math.abs(dirY);

    var maxX:Float = gridPosX * cellSize - x1;
    var maxY:Float = gridPosY * cellSize - y1;
    if (dirX >= 0) maxX += cellSize;
    if (dirY >= 0) maxY += cellSize;
    maxX /= dirX;
    maxY /= dirY;

    var stepX:Int = Maths.sign(dirX);
    var stepY:Int = Maths.sign(dirY);
    var gridGoalX:Int = getIndex(x2);
    var gridGoalY:Int = getIndex(y2);
    var currentDirX:Int = gridGoalX - gridPosX;
    var currentDirY:Int = gridGoalY - gridPosY;

    while (currentDirX * stepX > 0 || currentDirY * stepY > 0) {
    if (maxX < maxY) {
    maxX += deltaX;
    gridPosX += stepX;
    currentDirX = gridGoalX - gridPosX;
    } else {
    maxY += deltaY;
    gridPosY += stepY;
    currentDirY = gridGoalY - gridPosY;
    }

    if(!isValidGridPos(gridPosX, gridWidth) || !isValidGridPos(gridPosY, gridHeight)){
    break;
    }
    arr.push(getIndex1D(gridPosX, gridPosY));
    }

    return arr;
    }

    public function clear(){
    for (cell in grid) {
    if(cell.length > 0){
    for (b in cell) {
    b.gridIndex = [];
    for (co in cell) {
    co.gridIndex.splice(0, co.gridIndex.length);
    }
    cell = [];
    cell.splice(0, cell.length);
    }
    }
    }
  4. AndreiRudenko revised this gist Aug 5, 2015. 1 changed file with 11 additions and 0 deletions.
    11 changes: 11 additions & 0 deletions SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -175,4 +175,15 @@ class SpatialHash {
    }
    }
    }

    public function clear(){
    for (cell in grid) {
    if(cell.length > 0){
    for (b in cell) {
    b.gridIndex = [];
    }
    cell = [];
    }
    }
    }
    }
  5. AndreiRudenko revised this gist Jul 28, 2015. 1 changed file with 2 additions and 1 deletion.
    3 changes: 2 additions & 1 deletion SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -126,7 +126,8 @@ class SpatialHash {
    if(grid[i].length == 0) continue;
    // проверяем кто есть в ней
    for (cbd in grid[i]) {
    // add contacts here
    // add contacts here
    // addCollisionPair(_bd, cbd);
    }
    }
    }
  6. AndreiRudenko revised this gist Jul 28, 2015. 1 changed file with 1 addition and 1 deletion.
    2 changes: 1 addition & 1 deletion SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -1,6 +1,6 @@
    // SpatialHash uniform-grid implementation
    // Broad-phase algorithm for collision detection
    // Andrei Rudenko SpatialHash.hx (28.07.2015)
    // Andrei Rudenko // SpatialHash.hx (28.07.2015)

    import luxe.Vector;

  7. AndreiRudenko created this gist Jul 28, 2015.
    177 changes: 177 additions & 0 deletions SpatialHash.hx
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,177 @@
    // SpatialHash uniform-grid implementation
    // Broad-phase algorithm for collision detection
    // Andrei Rudenko SpatialHash.hx (28.07.2015)

    import luxe.Vector;

    class SpatialHash {
    /* the square cell gridLength of the grid. Must be larger than the largest shape in the space. */
    public var cellSize(default, null):Int;
    /* the world space width */
    public var gridWidth (default, null):Int;
    /* the world space height */
    public var gridHeight (default, null):Int;
    /* the number of buckets (i.e. cells) in the spatial grid */
    public var gridLength (default, null):Int;
    /* the array-list holding the spatial grid buckets */
    public var grid (default, null):haxe.ds.Vector<Array<Body>>;

    var invCellSize:Float;
    var gridWidthRes:Float;
    var gridHeightRes:Float;

    // for example: new SpatialHash(1280, 720, 32);
    public function new( _width:Float, _height:Float, _cellSize:Int) {

    gridWidthRes = _width;
    gridHeightRes = _height;

    cellSize = _cellSize;
    invCellSize = 1/cellSize;

    gridWidth = Math.ceil(_width * invCellSize);
    gridHeight = Math.ceil(_height * invCellSize);

    gridLength = Std.int(gridWidth * gridHeight);

    grid = new haxe.ds.Vector(gridLength);

    for (i in 0...gridLength) {
    grid[i] = new Array<Body>();
    }
    }

    public function addBody(b:Body){
    addIndex( b, 0, getGridPos(clampToGridPos(b.pos.clone())) );
    }

    public function removeBody(b:Body):Void{
    removeIndexes(b);
    }

    public function updateBody(b:Body){
    var arr:Array<Int> = aabbToGrid(b.aabb.min, b.aabb.max);
    removeIndexes(b);
    updateBodyIndexArrayLen(b.gridIndex, arr);
    moveIndexes(b, arr);
    getBodyCollision(b);
    }

    function getGridPos(_pos:Vector):Int { // i = x + w * y; x = i % w; y = i / w;
    return Std.int(Math.floor(_pos.x * invCellSize) + gridWidth * Math.floor(_pos.y * invCellSize) );
    }

    function getGridX(_ind:Int):Int { // x = i % w;
    return _ind % gridWidth;
    }

    function getGridY(_ind:Int):Int { // y = i / w;
    return Std.int(_ind / gridWidth);
    }

    function addIndex(_bd:Body, _bodyGridIndex:Int, _cellPos:Int){
    grid[_cellPos].push(_bd);
    _bd.gridIndex[_bodyGridIndex] = _cellPos;
    }

    function moveIndexes(_bd:Body, _ar:Array<Int>):Void{
    for (i in 0..._bd.gridIndex.length) {
    addIndex(_bd, i, _ar[i]);
    }
    }

    function removeIndexes(_bd:Body):Void{
    for (i in _bd.gridIndex) {
    removeIndex(_bd, i);
    }
    }

    function removeIndex(_bd:Body, _pos:Int) {
    reset_cell_debug(_pos);
    grid[_pos].remove(_bd);
    }

    function aabbToGrid(_min:Vector, _max:Vector):Array<Int> {

    clampToGridPos(_min);
    clampToGridPos(_max);
    var aabb_min:Int = getGridPos(_min);
    var aabb_max:Int = getGridPos(_max);

    var arr:Array<Int> = [];
    arr.push(aabb_min);
    arr.push(aabb_max);

    if(arr.length > 1){
    var lenX:Int = getGridX(arr[1]) - getGridX(arr[0]) + 1;
    var lenY:Int = getGridY(arr[1]) - getGridY(arr[0]) + 1;
    for (x in 0...lenX) {
    for (y in 0...lenY) {
    // пропускаем добавленые
    if((x == 0 && y == 0) || (x == lenX-1 && y == lenY-1) ){
    continue;
    }
    arr.push(x + gridWidth * y + arr[0]);
    }
    }
    }
    return arr;
    }

    function getBodyCollision(_bd:Body) {
    // добавление
    // проверяем все яйчейки
    for (i in _bd.gridIndex) {
    // проверяем есть ли ктото в этой яйчейке
    if(grid[i].length == 0) continue;
    // проверяем кто есть в ней
    for (cbd in grid[i]) {
    // add contacts here
    }
    }
    }

    function isValidGridPos(num:Int):Bool {
    if(num < 0 || num >= gridLength){
    return false;
    } else {
    return true;
    }
    }

    function clampToGrid(num:Int):Int {
    if( num < 0 ) {
    return 0;
    } else if(num >= gridLength) {
    return gridLength-1;
    } else {
    return num;
    }
    }

    function clampToGridPos(_vec:Vector):Vector {
    if(_vec.x < 0){
    _vec.x = 0;
    }
    if(_vec.y < 0){
    _vec.y = 0;
    }
    if(_vec.x >= gridWidthRes){
    _vec.x = gridWidthRes-1;
    }
    if(_vec.y >= gridHeightRes){
    _vec.y = gridHeightRes-1;
    }
    return _vec;
    }

    function updateBodyIndexArrayLen(gridAr:Array<Int>, ar:Array<Int>){
    while(gridAr.length != ar.length) {
    if(gridAr.length > ar.length){
    gridAr.pop();
    } else if (gridAr.length < ar.length){
    gridAr.push(0);
    }
    }
    }
    }