obsidian-visualiser/shared/physics.util.ts

import type { CanvasContent } from "~/types/canvas";
import { clamp } from "./general.utils";

export function intersects(aLeft: number, aTop: number, aRight: number, aBottom: number, bLeft: number, bTop: number, bRight: number, bBottom: number): boolean
{
    return aLeft <= bRight && aRight >= bLeft && aTop <= bBottom && aBottom >= bTop;
}

enum QuadConsts
{
    ENodeINext = 0,
    ENodeIElmt = 1,
    ElmtILft = 0, ElmtIRgt = 1, ElmtITop = 2, ElmtIBtm = 3,
    ElmtIId = 4,
    NodeIFirst = 0,
    NodeICount = 1,
    PropCount = 6,
    PropILft = 0, PropIRgt = 1, PropITop = 2, PropIBtm = 3,
    PropIIdx = 4,
    PropIDpt = 5,
}

const DEFAULT_SIZE = 128;
const MAX_BYTE_LENGTH = 1024*1024*16;

export interface AABB {
    x1: number;
    x2: number;
    y1: number;
    y2: number;
}

export class IntList
{
    #data: Int32Array;
    #fields: number;
    #capacity: number = DEFAULT_SIZE;
    #length: number = 0;
    #free: number = -1;

    constructor(fields: number)
    {
        if(fields <= 0)
            throw new Error("Invalid field count");

        this.#data = new Int32Array(new ArrayBuffer(this.#capacity * fields * Int32Array.BYTES_PER_ELEMENT, { maxByteLength: MAX_BYTE_LENGTH }));
        this.#fields = fields;
    }
    get length(): number
    {
        return this.#length;
    }

    get(index: number, field: number): number
    {
        if(index >= this.#length || index < 0)
            throw new Error("Invalid index");
        
        if(field >= this.#fields || field < 0)
            throw new Error("Invalid field");

        return this.#data[index*this.#fields + field];
    }
    set(index: number, field: number, value: number): void
    {
        if(index >= this.#length || index < 0)
            throw new Error("Invalid index");
        
        if(field >= this.#fields || field < 0)
            throw new Error("Invalid field");

        this.#data[index*this.#fields + field] = value;
    }
    clear(): void
    {
        //Never edit the array during clear, we'll keep every data as they *must* be override before being read.
        this.#length = 0;
        this.#free = -1;
    }
    push(): number
    {
        const pos = (this.#length + 1) * this.#fields;

        if(pos > this.#capacity * this.#fields)
        {
            this.#capacity *= 2;

            if((this.#data.buffer as ArrayBuffer).resizable)
            {
                (this.#data.buffer as ArrayBuffer).resize(clamp(this.#capacity * this.#fields * Int32Array.BYTES_PER_ELEMENT, 1, MAX_BYTE_LENGTH));
            }
            else
            {
                throw new Error("Cannot resize the buffer");
            }
        }

        return this.#length++;
    }
    pop(): void
    {
        if(this.#length <= 0)
            return;

        --this.#length;
    }
    insert(): number
    {
        if(this.#free !== -1)
        {
            const index = this.#free;
            this.#free = this.#data[index * this.#fields];
            return index;
        }
        else
            return this.push();
    }
    erase(index: number): void
    {
        if(index >= this.#length || index < 0)
            throw new Error("Invalid index");

        this.#data[index * this.#fields] = this.#free;
        this.#free = index;
    }

    toArray(): number[]
    {
        return [...this.#data.slice(0, this.#length)];
    }

    //DEBUG
    /* printReadable(names: string[] = []): void
    {
        const size = this.#length * this.#fields;
        for(let i = 0; i < size; i)
        {
            const obj: Record<string, number> = {};
            for(let j = 0; j < this.#fields; ++i, ++j)
            {
                obj[names[j] ?? j] = this.#data[i];
            }
            console.log(obj);
        }
    } */
}

const _process = new IntList(1);
const _nodes = new IntList(QuadConsts.PropCount);

/**
 * A node that contains elments is called a leaf. Its count is equals to the amount of children it holds.
 * A node that contains other nodes is called a branch. Its count is equals to -1.
 * The AABB of each node isn't stored and is computed on the go.
 */
export class Quadtree
{
    #bounds: AABB;
    #maxDepth: number = 8;
    #maxElmts: number = 4;
    #nodes: IntList = new IntList(2);
    #enodes: IntList = new IntList(2);
    #content: IntList = new IntList(5);

    #ids: string[] = [];

    ref: Ref<CanvasContent>;

    constructor(bounds: AABB, ref: Ref<CanvasContent>, maxDepth?: number, maxElmts?: number)
    {
        this.#bounds = bounds;
        this.#bounds.x1 = Math.round(this.#bounds.x1);
        this.#bounds.x2 = Math.round(this.#bounds.x2);
        this.#bounds.y1 = Math.round(this.#bounds.y1);
        this.#bounds.y2 = Math.round(this.#bounds.y2);

        this.#maxDepth = maxDepth ?? this.#maxDepth;
        this.#maxElmts = maxElmts ?? this.#maxElmts;

        this.ref = ref;

        this.#nodes.insert();
        this.#nodes.set(0, QuadConsts.NodeIFirst, -1);
        this.#nodes.set(0, QuadConsts.NodeICount, 0);
    }
    fetch(x: number, y: number): string[]
    {
        return this.query({x1: x, x2: x, y1: y, y2: y});
    }
    query(aabb: AABB): string[]
    {
        _process.clear();

        const leaves = this.#findLeaves(0, 0, this.#bounds.x1, this.#bounds.x2, this.#bounds.y1, this.#bounds.y2, Math.floor(aabb.x1), Math.floor(aabb.x2), Math.floor(aabb.y1), Math.floor(aabb.y2));

        const tmp: Record<number, boolean> = {};

        for(let i = 0; i < leaves.length; ++i)
        {
            const index = leaves.get(i, QuadConsts.PropIIdx);

            let node = this.#nodes.get(index, QuadConsts.NodeIFirst);
            while(node !== -1)
            {
                const elmt = this.#enodes.get(node, QuadConsts.ENodeIElmt);

                const idIndex = this.#content.get(elmt, QuadConsts.ElmtIId);
                const id = this.#ids[idIndex];
                const canvasNode = this.ref.value.nodes?.find(e => e.id === id)!;
                
                if (!tmp[elmt] && intersects(aabb.x1, aabb.y1, aabb.x2, aabb.y2, canvasNode.x, canvasNode.y, canvasNode.x + canvasNode.width, canvasNode.y + canvasNode.height))
                {
                    _process.set(_process.push(), 0, idIndex);
                    tmp[elmt] = true;
                }
                node = this.#enodes.get(node, QuadConsts.ENodeINext);
            }
        }

        return _process.toArray().map(e => this.#ids[e]);
    }
    insert(id: string, aabb: AABB): number
    {
        const index = this.#content.insert();
        const idIndex = this.#ids.push(id) - 1;

        this.#content.set(index, QuadConsts.ElmtILft, Math.floor(aabb.x1));
        this.#content.set(index, QuadConsts.ElmtIRgt, Math.floor(aabb.x2));
        this.#content.set(index, QuadConsts.ElmtITop, Math.floor(aabb.y1));
        this.#content.set(index, QuadConsts.ElmtIBtm, Math.floor(aabb.y2));
        this.#content.set(index, QuadConsts.ElmtIId, idIndex);

        this.#insertNode(0, 0, this.#bounds.x1, this.#bounds.x2, this.#bounds.y1, this.#bounds.y2, index);
        return index;
    }
    remove(index: number): void
    {
        if(index >= this.#content.length || index < 0)
            throw new Error("Provided index is out of bounds.");

        const leaves = this.#findLeaves(0, 0, this.#bounds.x1, this.#bounds.x2, this.#bounds.y1, this.#bounds.y2, this.#content.get(index, QuadConsts.ElmtILft), this.#content.get(index, QuadConsts.ElmtIRgt), this.#content.get(index, QuadConsts.ElmtITop), this.#content.get(index, QuadConsts.ElmtIBtm))

        for(let i = 0; i < leaves.length; ++i)
        {
            const prop = leaves.get(i, QuadConsts.PropIIdx);

            let node = this.#nodes.get(prop, QuadConsts.NodeIFirst), prev = -1;
            while(node !== -1 && this.#enodes.get(node, QuadConsts.ENodeIElmt) !== index)
            {
                prev = node;
                node = this.#enodes.get(node, QuadConsts.ENodeINext);
            }

            if(node !== -1)
            {
                const next = this.#enodes.get(node, QuadConsts.ENodeINext);

                if(prev == -1)
                    this.#nodes.set(prop, QuadConsts.NodeIFirst, next);
                else
                    this.#enodes.set(prev, QuadConsts.ENodeINext, next);

                this.#nodes.set(prop, QuadConsts.NodeICount, this.#nodes.get(prop, QuadConsts.NodeICount) - 1);
            }
        }

        this.#content.erase(index);
    }
    clear(): void
    {
        this.#nodes.clear();
        this.#content.clear();
        this.#enodes.clear();
        this.#ids.length = 0;

        this.#nodes.insert();
        this.#nodes.set(0, QuadConsts.NodeIFirst, -1);
        this.#nodes.set(0, QuadConsts.NodeICount, 0);
    }
    cleanup(): void
    {
        let updated = false;
        _process.clear();

        if(this.#nodes.get(0, QuadConsts.NodeICount) === -1)
            _process.set(_process.push(), 0, 0);

        while(_process.length > 0)
        {
            const node = _process.get(_process.length - 1, 0);
            const fc = this.#nodes.get(node, QuadConsts.NodeIFirst);

            let empty = 0;
            _process.pop();

            for(let i = 0; i < 4; ++i)
            {
                const current = fc + i;

                const count = this.#nodes.get(current, QuadConsts.NodeICount);

                if(count === 0) //Count the amount of empty leaves
                    ++empty
                else if(count === -1) //Add this node to the check process if it's a branch
                    _process.set(_process.push(), 0, current);
            }

            if(empty === 4)
            {
                //Because of the way the IntList is made, it's preferable to erase in the reversed order as the last erased index becomes the first available index.
                this.#nodes.erase(fc + 3);
                this.#nodes.erase(fc + 2);
                this.#nodes.erase(fc + 1);
                this.#nodes.erase(fc + 0);

                //The branch becomes a empty leaf
                this.#nodes.set(node, QuadConsts.NodeICount, 0);
                this.#nodes.set(node, QuadConsts.NodeIFirst, -1);

                updated = true;
            }
        }
    }
    traverse(cb: (index: number, depth: number, left: number, top: number, right: number, bottom: number, leaf: boolean) => void): void
    {
        _nodes.clear();

        insert(_nodes, 0, 0, this.#bounds.x1, this.#bounds.x2, this.#bounds.y1, this.#bounds.y2);

        while(_nodes.length > 0)
        {
            const last = _nodes.length - 1;

            const node = _nodes.get(last, QuadConsts.PropIIdx);
            const left = _nodes.get(last, QuadConsts.PropILft);
            const right = _nodes.get(last, QuadConsts.PropIRgt);
            const top = _nodes.get(last, QuadConsts.PropITop);
            const bottom = _nodes.get(last, QuadConsts.PropIBtm);
            const depth = _nodes.get(last, QuadConsts.PropIDpt);
            
            _nodes.pop();

            const count = this.#nodes.get(node, QuadConsts.NodeICount);

            cb(node, depth, left, top, right, bottom, count !== -1);

            //If it's a branch
            if (count === -1)
            {
                const fc = this.#nodes.get(node, QuadConsts.NodeIFirst);
                const mx = left + (right - left) / 2, my = top + (bottom - top) / 2;

                insert(_nodes, fc + 0, depth + 1, left, mx, top, my);
                insert(_nodes, fc + 1, depth + 1, mx, right, top, my);
                insert(_nodes, fc + 2, depth + 1, left, mx, my, bottom);
                insert(_nodes, fc + 3, depth + 1, mx, right, my, bottom);
            }
        }
    }

    #findLeaves(index: number, depth: number, lleft: number, lright: number, ltop: number, lbottom: number, eleft: number, eright: number, etop: number, ebottom: number): IntList
    {
        const leaves = new IntList(QuadConsts.PropCount);
        _nodes.clear();
        insert(_nodes, index, depth, lleft, lright, ltop, lbottom);
    
        while (_nodes.length > 0)
        {
            const last = _nodes.length - 1;

            const nodeLeft = _nodes.get(last, QuadConsts.PropILft);
            const nodeRight = _nodes.get(last, QuadConsts.PropIRgt);
            const nodeTop = _nodes.get(last, QuadConsts.PropITop);
            const nodeBottom = _nodes.get(last, QuadConsts.PropIBtm);
            const nodeIndex = _nodes.get(last, QuadConsts.PropIIdx);
            const nodeDepth = _nodes.get(last, QuadConsts.PropIDpt);
            
            _nodes.pop();

            if(this.#nodes.get(nodeIndex, QuadConsts.NodeICount) !== -1)
                insert(leaves, nodeIndex, nodeDepth, nodeLeft, nodeRight, nodeTop, nodeBottom);
            else
            {
                const fc = this.#nodes.get(nodeIndex, QuadConsts.NodeIFirst);

                const mx = nodeLeft + (nodeRight - nodeLeft) / 2, my = nodeTop + (nodeBottom - nodeTop) / 2;
                
                if(etop <= my)
                {
                    if(eleft <= mx) //Add a new 
                        insert(_nodes, fc + 0, nodeDepth + 1, nodeLeft, mx, nodeTop, my); 
                    if(eright > mx)
                        insert(_nodes, fc + 1, nodeDepth + 1, mx, nodeRight, nodeTop, my); 
                }
                if(ebottom > my)
                {
                    if(eleft <= mx)
                        insert(_nodes, fc + 2, nodeDepth + 1, nodeLeft, mx, my, nodeBottom); 
                    if(eright > mx)
                        insert(_nodes, fc + 3, nodeDepth + 1, mx, nodeRight, my, nodeBottom); 
                }
            }
        }

        return leaves;
    }
    #insertLeaf(index: number, depth: number, left: number, top: number, right: number, bottom: number, elmt: number): void
    {
        const fc = this.#nodes.get(index, QuadConsts.NodeIFirst);
        this.#nodes.set(index, QuadConsts.NodeIFirst, this.#enodes.insert());
        this.#enodes.set(this.#nodes.get(index, QuadConsts.NodeIFirst), QuadConsts.ENodeINext, fc);
        this.#enodes.set(this.#nodes.get(index, QuadConsts.NodeIFirst), QuadConsts.ENodeIElmt, elmt);

        if(this.#nodes.get(index, QuadConsts.NodeICount) === this.#maxElmts && depth < this.#maxDepth)
        {
            _process.clear();
            //For each element of the leaf
            while(this.#nodes.get(index, QuadConsts.NodeIFirst) !== -1)
            {
                const current = this.#nodes.get(index, QuadConsts.NodeIFirst);
                const next = this.#enodes.get(current, QuadConsts.ENodeINext);
                const value = this.#enodes.get(current, QuadConsts.ENodeIElmt);

                 //Remove the element
                this.#nodes.set(index, QuadConsts.NodeIFirst, next);
                this.#enodes.erase(current);

                //Store it for reprocess
                _process.set(_process.push(), 0, value);
            }

            const newFc = this.#nodes.insert();
            this.#nodes.set(newFc + 0, QuadConsts.NodeIFirst, -1);
            this.#nodes.set(newFc + 0, QuadConsts.NodeICount, 0);

            this.#nodes.insert();
            this.#nodes.set(newFc + 1, QuadConsts.NodeIFirst, -1);
            this.#nodes.set(newFc + 1, QuadConsts.NodeICount, 0);

            this.#nodes.insert();
            this.#nodes.set(newFc + 2, QuadConsts.NodeIFirst, -1);
            this.#nodes.set(newFc + 2, QuadConsts.NodeICount, 0);

            this.#nodes.insert();
            this.#nodes.set(newFc + 3, QuadConsts.NodeIFirst, -1);
            this.#nodes.set(newFc + 3, QuadConsts.NodeICount, 0);

            this.#nodes.set(index, QuadConsts.NodeIFirst, newFc);

            this.#nodes.set(index, QuadConsts.NodeICount, -1);

            for(let i = 0; i < _process.length; ++i)
                this.#insertNode(index, depth, left, right, top, bottom, _process.get(i, 0));
        }
        else
        {
            this.#nodes.set(index, QuadConsts.NodeICount, this.#nodes.get(index, QuadConsts.NodeICount) + 1);
        }
    }
    #insertNode(index: number, depth: number, left: number, right: number, top: number, bottom: number, elmt: number): void
    {
        const leaves = this.#findLeaves(index, depth, left, right, top, bottom, this.#content.get(elmt, QuadConsts.ElmtILft), this.#content.get(elmt, QuadConsts.ElmtIRgt), this.#content.get(elmt, QuadConsts.ElmtITop), this.#content.get(elmt, QuadConsts.ElmtIBtm));

        for(let i = 0; i < leaves.length; ++i)
            this.#insertLeaf(leaves.get(i, QuadConsts.PropIIdx), leaves.get(i, QuadConsts.PropIDpt), leaves.get(i, QuadConsts.PropILft), leaves.get(i, QuadConsts.PropITop), leaves.get(i, QuadConsts.PropIRgt), leaves.get(i, QuadConsts.PropIBtm), elmt);
    }
}

function insert(list: IntList, index: number, depth: number, left: number, right: number, top: number, bottom: number): void
{
    const idx = list.push();
    list.set(idx, QuadConsts.PropILft, left);
    list.set(idx, QuadConsts.PropIRgt, right);
    list.set(idx, QuadConsts.PropITop, top);
    list.set(idx, QuadConsts.PropIBtm, bottom);
    list.set(idx, QuadConsts.PropIIdx, index);
    list.set(idx, QuadConsts.PropIDpt, depth);
}