constraint-solver-js/build/solver.js

2D rigid body constraint solver written in typescript.

import { Differentiator } from "fsolve-js";
import { abs, add, index, inv, max, multiply, range, sign } from "mathjs";
import { SolverSolution } from "./solver-solution.js";
export class Solver {
    /**
     * Solver constructor
     * @param stopError - Minimum error to stop solver.
     * @param maxIterations - Maximum number of iterations to stop solver.
     * @param timeOut - Solver timeout (in ms)
     * @param delta Numerical differentiation delta.
     */
    constructor(stopError = 1e-6, maxIterations = 1e3, timeOut = 3.6e6, delta = 1e-9) {
        this.stopError = stopError;
        this.maxIterations = maxIterations;
        this.timeOut = timeOut;
        this.delta = delta;
        this.diff = new Differentiator(delta);
    }
    solve(f, x) {
        const n = x.size()[0];
        const m = f(x).size()[0];
        if (n === m) {
            return this.solveDetermined(f, x);
        }
        else if (n > m) {
            return this.solveUnderdetermined(f, x);
        }
        else {
            throw new Error("Unable to solve. System has more equations than variables");
        }
    }
    solveUnderdetermined(f, x) {
        const n = x.size()[0];
        const m = f(x).size()[0];
        if (m > n) {
            throw new Error("Unable to solve. System has more equations than variables.");
        }
        if (m == n) {
            console.warn("System has as many variables as equations. Use solveDetermined for better performance.");
        }
        let error = Infinity;
        let iter = 0;
        const initialTime = Date.now();
        // const selIndices = this.selectIndices(f, x);
        while (error >= this.stopError) {
            const selIndices = this.selectIndices(f, x);
            const y = f(x);
            const J = this.diff.jacobian(f, x);
            const smallJ = J.subset(index(range(0, m), selIndices));
            let smallX = x.subset(index(selIndices, 0));
            if (iter >= this.maxIterations) {
                SolverSolution.maxIterReached(x);
            }
            else if ((Date.now() - initialTime) >= this.timeOut) {
                SolverSolution.timeOut(x);
            }
            const b = multiply(y, -1);
            const invJ = inv(smallJ);
            const h = multiply(invJ, b);
            smallX = add(smallX, h);
            // update x
            selIndices.forEach((bigXi, smallXi) => {
                const xi = smallX.get([smallXi, 0]);
                x.set([bigXi, 0], xi);
            });
            error = max(abs(f(x)));
            iter += 1;
        }
        return SolverSolution.success(x);
    }
    solveDetermined(f, x) {
        const n = x.size()[0];
        const m = f(x).size()[0];
        if (m !== n) {
            throw new Error("Unable to solve. System has different number of equations and variables.");
        }
        let error = Infinity;
        let iter = 0;
        const initialTime = Date.now();
        while (error >= this.stopError) {
            if (iter >= this.maxIterations) {
                return SolverSolution.maxIterReached(x);
            }
            else if ((Date.now() - initialTime) >= this.timeOut) {
                return SolverSolution.timeOut(x);
            }
            const y0 = f(x);
            const J = this.diff.jacobian(f, x);
            const b = multiply(y0, -1);
            const invJ = inv(J);
            const h = multiply(invJ, b);
            x = add(x, h);
            error = max(abs(f(x)));
            iter += 1;
        }
        return SolverSolution.success(x);
    }
    selectIndices(f, x) {
        const J = this.diff.jacobian(f, x);
        const m = J.size()[1];
        const rowSums = this.sumRows(abs(sign(J)));
        rowSums.sort((a, b) => {
            return a.sum - b.sum;
        });
        const selectedIndicesDict = {};
        const selectedIndices = [];
        rowSums.forEach((row) => {
            const i = row.i;
            let selectedjs = [];
            for (let j = 0; j < m; j++) {
                const absJij = Math.abs(J.get([i, j]));
                let max = 0;
                if (absJij > max && (!selectedIndicesDict[j])) {
                    selectedjs.push(j);
                    max = absJij;
                }
            }
            if (selectedjs.length === 0) {
                throw new Error("Unale to find enough independent variables to solve the system.");
            }
            const selectedj = selectedjs[Math.floor(Math.random() * selectedjs.length)];
            selectedIndicesDict[selectedj] = true;
            selectedIndices.push(selectedj);
        });
        selectedIndices.sort();
        return selectedIndices;
    }
    sumRows(mat) {
        const n = mat.size()[0];
        const m = mat.size()[1];
        const ret = [];
        for (let i = 0; i < n; i++) {
            let sum = 0;
            for (let j = 0; j < m; j++) {
                sum += mat.get([i, j]);
            }
            ret.push({ sum: sum, i: i });
        }
        return ret;
    }
}