@woosh/meep-engine/src/core/geom/packing/miniball/Subspan.js

Pure JavaScript game engine. Fully featured and production ready.

/**
 * @author Alex Goldring 14/05/2018 - Ported to JS using JSweet + manual editing
 * @author Kaspar Fischer (hbf) 09/05/2013
 * @source https://github.com/hbf/miniball
 * @generated Generated from Java with JSweet 2.2.0-SNAPSHOT - http://www.jsweet.org
 */

import { assert } from "../../../assert.js";
import { BitSet } from "../../../binary/BitSet.js";
import { BinaryDataType } from "../../../binary/type/BinaryDataType.js";
import { SquareMatrix } from "../../../math/matrix/SquareMatrix.js";
import { vector_dot_offset } from "../../vec/vector_dot_offset.js";
import { vector_axpy_offset } from "../../vec/vector_axpy_offset.js";
import { givens_rotation_coefficients } from "../../../math/linalg/givens/givens_rotation_coefficients.js";
import { givens_apply_rows_offset } from "../../../math/linalg/givens/givens_apply_rows_offset.js";

export class Subspan {
    /**
     *
     * @param {number} dim dimension count
     * @param {PointSet} s
     * @param {number} k
     */
    constructor(dim, s, k) {
        assert.isNonNegativeInteger(dim, 'dim');
        assert.defined(s, 's');
        assert.isNonNegativeInteger(k, 'k');

        /**
         * @type {PointSet}
         */
        this.S = s;

        /**
         * reserve bit-field size
         * @type {BitSet}
         */
        this.membership = BitSet.fixedSize(s.size());

        /**
         *
         * @type {number}
         */
        this.dim = dim;

        /**
         *
         * @type {Int32Array}
         */
        this.members = new Int32Array(dim + 1);


        // Q and R are dim x dim, column-major (per SquareMatrix). The k-th
        // column lives contiguously at offsets [k*dim, (k+1)*dim). Inner
        // loops index the flat .data buffer directly with offset arithmetic
        // rather than going through subarray views — see vector_dot_offset
        // and friends.
        /**
         * @type {SquareMatrix}
         */
        this.Q = new SquareMatrix(dim, BinaryDataType.Float64);
        this.Q.eye();

        /**
         * @type {SquareMatrix}
         */
        this.R = new SquareMatrix(dim, BinaryDataType.Float64);

        /**
         * D sized vector
         * @type {Float64Array}
         */
        this.u = new Float64Array(dim);
        /**
         * D sized vector
         * @type {Float64Array}
         */
        this.w = new Float64Array(dim);

        this.r = 0;

        /**
         * Givens rotation coefficients scratch: cs[0] = c, cs[1] = s.
         * Holding this as an instance field avoids per-call allocation of a
         * fresh 2-element array — see givens_rotation_coefficients's contract.
         * @type {Float64Array}
         */
        this.cs = new Float64Array(2);


        this.membership.set(k, true);
        this.members[this.r] = k;
    }

    dimension() {
        return this.dim;
    }

    /**
     * The size of the instance's set <i>M</i>, a number between 0 and `dim+1`.
     *
     * Complexity: O(1).
     *
     * @returns {int} <i>|M|</i>
     */
    size() {
        return this.r + 1;
    }

    /**
     * Whether <i>S[i]</i> is a member of <i>M</i>.
     *
     * Complexity: O(1)
     *
     * @param {int} i
     *          the "global" index into <i>S</i>
     * @returns {boolean} true iff <i>S[i]</i> is a member of <i>M</i>
     */
    isMember(i) {
        return this.membership.get(i);
    }

    /**
     * The global index (into <i>S</i>) of an arbitrary element of <i>M</i>.
     *
     * Precondition: `size()>0`
     *
     * Postcondition: `isMember(anyMember())`
     * @returns {number}
     */
    anyMember() {
        return this.members[this.r];
    }

    /**
     * The index (into <i>S</i>) of the <i>i</i>th point in <i>M</i>. The points in <i>M</i> are
     * internally ordered (in an arbitrary way) and this order only changes when {@link add()} or
     * {@link remove()} is called.
     *
     * Complexity: O(1)
     *
     * @param {number} i
     * the "local" index, 0 ≤ i < `size()`
     * @return {number} <i>j</i> such that <i>S[j]</i> equals the <i>i</i>th point of M
     */
    globalIndex(i) {
        return this.members[i];
    }

    /**
     * Short-hand for code readability to access element <i>(i,j)</i> of a matrix that is stored in a
     * one-dimensional array.
     *
     * @param {number} i
     * zero-based row number
     * @param {number} j
     * zero-based column number
     * @return {number} the index into the one-dimensional array to get the element at position <i>(i,j)</i> in
     * the matrix
     * @private
     */
    ind(i, j) {
        return i * this.dim + j;
    }

    /**
     * The point `members[r]` is called the <i>origin</i>.
     *
     * @return {number} index into <i>S</i> of the origin.
     * @private
     */
    origin() {
        return this.members[this.r];
    }

    /**
     * Appends the new column <i>u</i> (which is a member field of this instance) to the right of <i>A
     * = QR</i>, updating <i>Q</i> and <i>R</i>. It assumes <i>r</i> to still be the old value, i.e.,
     * the index of the column used now for insertion; <i>r</i> is not altered by this routine and
     * should be changed by the caller afterwards.
     *
     * Precondition: `r<dim`
     * @private
     */
    appendColumn() {
        const dim = this.dim;

        const u = this.u;
        const Q = this.Q.data;
        const R = this.R.data;
        const cs = this.cs;

        const rOff = this.r * dim;

        for (let i = 0; i < dim; ++i) {
            R[rOff + i] = vector_dot_offset(Q, i * dim, u, 0, dim);
        }

        for (let j = dim - 1; j > this.r; --j) {

            givens_rotation_coefficients(R[rOff + j - 1], R[rOff + j], cs);
            const c = cs[0];
            const s = cs[1];

            R[rOff + j - 1] = c * R[rOff + j - 1] + s * R[rOff + j];

            givens_apply_rows_offset(Q, (j - 1) * dim, j * dim, c, s, dim);
        }

    }

    /**
     * Adds the point <i>S[index]</i> to the instance's set <i>M</i>.
     *
     * Precondition: `!isMember(index)`
     *
     * Complexity: O(dim^2).
     *
     * @param {number} index index into <i>S</i> of the point to add
     */
    add(index) {
        let o = this.origin();

        const dim = this.dim;

        const u = this.u;
        const S = this.S;

        for (let i = 0; i < dim; ++i) {
            u[i] = S.coord(index, i) - S.coord(o, i);
        }

        this.appendColumn();

        this.membership.set(index, true);

        this.members[this.r + 1] = this.members[this.r];
        this.members[this.r] = index;

        ++this.r;
    }

    /**
     * Computes the vector <i>w</i> directed from point <i>p</i> to <i>v</i>, where <i>v</i> is the
     * point in <i>aff(M)</i> that lies nearest to <i>p</i>.
     *
     * Precondition: `size()>0`
     *
     * Complexity: O(dim^2)
     *
     * @param {number[]} p
     * @param {number[]} w Euclidean coordinates of point <i>p</i>
     * @return {number} the squared length of <i>w</i>
     */
    shortestVectorToSpan(p, w) {
        const o = this.origin();

        const dim = this.dim;

        const S = this.S;

        for (let i = 0; i < dim; ++i) {
            w[i] = S.coord(o, i) - p[i];
        }

        const Q = this.Q.data;

        const r = this.r;

        for (let j = 0; j < r; ++j) {

            const qOff = j * dim;

            const scale = vector_dot_offset(w, 0, Q, qOff, dim);

            vector_axpy_offset(w, 0, -scale, Q, qOff, dim);

        }

        return vector_dot_offset(w, 0, w, 0, dim);
    }

    /**
     * Use this for testing only; the method allocates additional storage and copies point
     * coordinates.
     * @return {number}
     */
    representationError() {

        const size = this.size();

        const lambdas = new Float64Array(size);

        const dim = this.dim;

        const pt = new Float64Array(dim);

        let max = 0;

        for (let j = 0; j < size; ++j) {
            for (let i = 0; i < dim; ++i) {
                pt[i] = this.S.coord(this.globalIndex(j), i);
            }

            this.findAffineCoefficients(pt, lambdas);

            let error = Math.abs(lambdas[j] - 1.0);

            if (error > max) {
                max = error;
            }

            for (let i = 0; i < j; ++i) {
                error = Math.abs(lambdas[i]);
                if (error > max) {
                    max = error;
                }
            }

            // skip over lambdas[j]

            for (let i = j + 1; i < size; ++i) {
                error = Math.abs(lambdas[i]);
                if (error > max) {
                    max = error;
                }
            }

        }

        return max;
    }

    /**
     * Calculates the `size()`-many coefficients in the representation of <i>p</i> as an affine
     * combination of the points <i>M</i>.
     *
     * The <i>i</i>th computed coefficient `lambdas[i] `corresponds to the <i>i</i>th point in
     * <i>M</i>, or, in other words, to the point in <i>S</i> with index `globalIndex(i)`.
     *
     * Complexity: O(dim^2)
     *
     * Preconditions: c lies in the affine hull aff(M) and size() > 0.
     * @param {number[]|Float64Array} p
     * @param {number[]|Float64Array} lambdas
     */
    findAffineCoefficients(p, lambdas) {
        let o = this.origin();

        const dim = this.dim;
        const u = this.u;

        for (let i = 0; i < dim; ++i) {
            u[i] = p[i] - this.S.coord(o, i);
        }

        const w = this.w;
        const Q = this.Q.data;

        for (let i = 0; i < dim; ++i) {
            w[i] = vector_dot_offset(Q, i * dim, u, 0, dim);
        }

        let origin_lambda = 1;

        const R = this.R.data;
        const r = this.r;

        for (let j = r - 1; j >= 0; --j) {

            const jjOff = j * dim;

            for (let k = j + 1; k < r; ++k) {
                w[j] -= lambdas[k] * R[k * dim + j];
            }

            const lj = w[j] / R[jjOff + j];

            lambdas[j] = lj;
            origin_lambda -= lj;
        }

        lambdas[r] = origin_lambda;
    }

    /**
     * Given <i>R</i> in lower Hessenberg form with subdiagonal entries 0 to `pos-1` already all
     * zero, clears the remaining subdiagonal entries via Givens rotations.
     * @param {number} pos
     * @private
     */
    hessenberg_clear(pos) {

        let cursor = pos;

        const R = this.R.data;
        const Q = this.Q.data;

        const dim = this.dim;
        const cs = this.cs;
        const r = this.r;

        for (; cursor < r; ++cursor) {

            const cOff = cursor * dim;

            givens_rotation_coefficients(R[cOff + cursor], R[cOff + cursor + 1], cs);
            const c = cs[0];
            const s = cs[1];

            R[cOff + cursor] = c * R[cOff + cursor] + s * R[cOff + cursor + 1];

            for (let j = cursor + 1; j < r; ++j) {

                const jOff = j * dim;
                const a = R[jOff + cursor];
                const b = R[jOff + cursor + 1];

                R[jOff + cursor] = c * a + s * b;
                R[jOff + cursor + 1] = c * b - s * a;
            }

            givens_apply_rows_offset(Q, cOff, cOff + dim, c, s, dim);
        }

    }

    /**
     * Update current QR-decomposition <i>A = QR</i> to
     *
     * <pre>
     * A + u * [1,...,1] = Q' R'.
     * </pre>
     * @private
     */
    special_rank_1_update() {
        const dim = this.dim;

        const w = this.w;
        const u = this.u;

        const Q = this.Q.data;
        const R = this.R.data;
        const cs = this.cs;
        const r = this.r;

        for (let i = 0; i < dim; ++i) {
            w[i] = vector_dot_offset(Q, i * dim, u, 0, dim);
        }

        for (let k = dim - 1; k > 0; --k) {

            givens_rotation_coefficients(w[k - 1], w[k], cs);
            const c = cs[0];
            const s = cs[1];

            w[k - 1] = c * w[k - 1] + s * w[k];

            const prevOff = (k - 1) * dim;
            R[prevOff + k] = -s * R[prevOff + k - 1];
            R[prevOff + k - 1] *= c;

            for (let j = k; j < r; ++j) {
                const jOff = j * dim;
                const a = R[jOff + k - 1];
                const b = R[jOff + k];
                R[jOff + k - 1] = c * a + s * b;
                R[jOff + k] = c * b - s * a;
            }

            givens_apply_rows_offset(Q, prevOff, prevOff + dim, c, s, dim);
        }

        const w0 = w[0];
        for (let j = 0; j < r; ++j) {
            R[j * dim] += w0;
        }

        this.hessenberg_clear(0);
    }

    /**
     *
     * @param {number} index
     */
    remove(index) {
        this.membership.clear(this.globalIndex(index));

        if (index === this.r) {
            let o = this.origin();
            let gi = this.globalIndex(this.r - 1);

            const dim = this.dim;
            const S = this.S;

            const u = this.u;

            for (let i = 0; i < dim; ++i) {
                u[i] = S.coord(o, i) - S.coord(gi, i);
            }

            --this.r;

            this.special_rank_1_update();
        } else {
            const R = this.R.data;
            const dim = this.dim;

            const members = this.members;

            // Shift the column block [index+1 .. r-1] down by one position.
            // Each column is `dim` contiguous floats in column-major storage,
            // so the whole shift is a single copyWithin call.
            const dstOff = index * dim;
            const srcOff = (index + 1) * dim;
            const endOff = this.r * dim;
            R.copyWithin(dstOff, srcOff, endOff);

            for (let j = index + 1; j < this.r; ++j) {
                members[j - 1] = members[j];
            }

            members[this.r - 1] = members[this.r];
            --this.r;

            this.hessenberg_clear(index);
        }
    }
}