Examples with DistanceGrid maspack.geometry.DistanceGrid used on opensource projects

Example 11 with DistanceGrid

use of maspack.geometry.DistanceGrid in project artisynth_core by artisynth.

the class RigidBody method updatePosState.

@Override
protected void updatePosState() {
    updateAttachmentPosStates();
    PolygonalMesh mesh = getMesh();
    if (mesh != null) {
        mesh.setMeshToWorld(myState.XFrameToWorld);
        PolygonalMesh surf = null;
        if (myRenderDistanceSurface != myRenderDistanceSurface.NONE && (surf = getDistanceSurface()) != null) {
            surf.setMeshToWorld(myState.XFrameToWorld);
        }
        DistanceGrid grid = null;
        if (hasDistanceGrid() && (grid = mySDGrid) != null) {
            grid.setLocalToWorld(myState.XFrameToWorld);
        }
    }
}

Example 12 with DistanceGrid

use of maspack.geometry.DistanceGrid in project artisynth_core by artisynth.

the class RigidMeshComp method getDistanceGrid.

@Override
public DistanceGrid getDistanceGrid() {
    if (getMesh() instanceof PolygonalMesh) {
        if (mySDGrid == null) {
            List<Face> faces = ((PolygonalMesh) getMesh()).getFaces();
            mySDGrid = new DistanceGrid(faces, myGridMargin, myMaxGridDivisions, /*signed=*/
            true);
        }
    } else {
        mySDGrid = null;
    }
    return mySDGrid;
}

Example 13 with DistanceGrid

use of maspack.geometry.DistanceGrid in project artisynth_core by artisynth.

the class DistanceGridTest method test.

public void test() {
    double EPS = 1e-14;
    PolygonalMesh torus = MeshFactory.createTorus(1.0, 0.5, 24, 24);
    PolygonalMesh torusT = torus.copy();
    RigidTransform3d TGL = new RigidTransform3d();
    RigidTransform3d TLW = new RigidTransform3d();
    RigidTransform3d TGW = new RigidTransform3d();
    TGL.setRandom();
    TLW.setRandom();
    TGW.mul(TLW, TGL);
    torusT.transform(TGL);
    Vector3i resolution = new Vector3i(20, 20, 10);
    DistanceGrid grid = new DistanceGrid(torus.getFaces(), 0.1, resolution, /*signed=*/
    true);
    grid.setLocalToWorld(TLW);
    Vector3d widths = new Vector3d();
    Vector3d center = new Vector3d();
    grid.getWidths(widths);
    grid.getCenter(center);
    // For now, just set the distances, since precision issues can result in
    // differing distances computed during the sweep process
    DistanceGrid gridT = new DistanceGrid(widths, resolution, TGL);
    gridT.setDistances(grid.getDistances(), /*signed=*/
    true);
    gridT.setLocalToWorld(TLW);
    int numtests = 1000;
    for (int i = 0; i < numtests; i++) {
        Point3d q = new Point3d(RandomGenerator.nextDouble(0, widths.x), RandomGenerator.nextDouble(0, widths.y), RandomGenerator.nextDouble(0, widths.z));
        q.add(center);
        q.scaledAdd(-0.5, widths);
        Point3d qw = new Point3d(q);
        qw.transform(TLW);
        Point3d qt = new Point3d();
        qt.transform(TGL, q);
        Point3d qwt = new Point3d(qt);
        qwt.transform(TLW);
        Vector3d nrm = new Vector3d();
        Matrix3d Dnrm = new Matrix3d();
        Vector3d nrmW = new Vector3d();
        Matrix3d DnrmW = new Matrix3d();
        double d = grid.getLocalDistanceAndNormal(nrm, Dnrm, q);
        double dw = grid.getWorldDistanceAndNormal(nrmW, qw);
        double dchk = distanceCheck(grid, q);
        checkEquals("localDistanceAndNormal, d=", d, dchk, EPS);
        checkEquals("worldDistanceAndNormal, d=", dw, dchk, EPS);
        if (d != DistanceGrid.OUTSIDE_GRID) {
            // check Dnrm
            Matrix3d Dchk = computeNumericNormalDerivative(grid, q);
            Matrix3d Err = new Matrix3d();
            Err.sub(Dchk, Dnrm);
            double err = Err.frobeniusNorm() / Dnrm.frobeniusNorm();
            if (err > 1e-7) {
                System.out.println("Dchk=\n" + Dchk.toString("%12.8f"));
                System.out.println("Dnrm=\n" + Dnrm.toString("%12.8f"));
                System.out.println("Err=\n" + Err.toString("%12.8f"));
                throw new TestException("Dnrm gradient error computation: err=" + err);
            }
            Vector3d NchkW = new Vector3d(nrm);
            NchkW.transform(TLW);
            if (!NchkW.epsilonEquals(nrmW, 1e-12)) {
                System.out.println("nrmW=\n" + nrmW.toString("%12.8f"));
                System.out.println("chkW=\n" + NchkW.toString("%12.8f"));
                throw new TestException("error computing world normal");
            }
        }
        Vector3d nrmT = new Vector3d();
        double dt = gridT.getLocalDistanceAndNormal(nrmT, qt);
        if ((dt == DistanceGrid.OUTSIDE_GRID) != (d == DistanceGrid.OUTSIDE_GRID)) {
            System.out.println("grid outside=" + (d == DistanceGrid.OUTSIDE_GRID));
            System.out.println("transformed grid outside=" + (dt == DistanceGrid.OUTSIDE_GRID));
            throw new TestException("Outside differs for grid and transformed grid");
        }
        if (dt != DistanceGrid.OUTSIDE_GRID) {
            if (Math.abs(d - dt) > EPS) {
                System.out.println("grid d=" + d);
                System.out.println("transformed grid d=" + dt);
                throw new TestException("Distance differs for grid and transformed grid");
            }
        }
        Vector3d grad = new Vector3d();
        Vector3d gradW = new Vector3d();
        Vector3d gradT = new Vector3d();
        Vector3d gradWT = new Vector3d();
        d = grid.getLocalDistanceAndGradient(grad, q);
        checkEquals("localDistanceAndGradient, d=", d, dchk, EPS);
        if (d != DistanceGrid.OUTSIDE_GRID) {
            // check grad
            Vector3d gchk = computeNumericGradient(grid, q);
            Vector3d gerr = new Vector3d();
            gerr.sub(gchk, grad);
            double err = gerr.norm() / gchk.norm();
            if (err > 1e-7) {
                System.out.println("gchk=\n" + gchk.toString("%12.8f"));
                System.out.println("grad=\n" + grad.toString("%12.8f"));
                throw new TestException("Distance gradient error computation: err=" + err);
            }
        }
        Matrix3d Dgrad = new Matrix3d();
        Matrix3d DgradW = new Matrix3d();
        Matrix3d DgradT = new Matrix3d();
        Matrix3d DgradWT = new Matrix3d();
        d = grid.getQuadDistance(q);
        dchk = quadDistanceCheck(grid, q);
        dw = grid.getWorldQuadDistance(qw);
        checkEquals("getQuadDistance, d=", d, dchk, EPS);
        checkEquals("getWorldQuadDistance, d=", dw, dchk, EPS);
        d = gridT.getQuadDistance(qt);
        dw = gridT.getWorldQuadDistance(qwt);
        checkEquals("getQuadDistanceT, d=", d, dchk, EPS);
        checkEquals("getWorldQuadDistanceT, d=", dw, dchk, EPS);
        d = grid.getQuadDistanceAndGradient(grad, Dgrad, q);
        dw = grid.getWorldQuadDistanceAndGradient(gradW, DgradW, qw);
        checkEquals("getQuadDistanceAndGradient, d=", d, dchk, EPS);
        checkEquals("getWorldQuadDistanceAndGradient, d=", dw, dchk, EPS);
        d = gridT.getQuadDistanceAndGradient(gradT, DgradT, qt);
        dw = gridT.getWorldQuadDistanceAndGradient(gradWT, DgradWT, qwt);
        checkEquals("getQuadDistanceAndGradientT, d=", d, dchk, EPS);
        checkEquals("getWorldQuadDistanceAndGradientT, d=", dw, dchk, EPS);
        if (d != DistanceGrid.OUTSIDE_GRID) {
            // check grad
            Vector3d gchk = computeNumericQuadGradient(grid, q);
            checkEquals("grad in getQuadDistanceAndGradient:", grad, gchk, grad.norm() * 5e-7);
            gchk.transform(TLW.R, grad);
            checkEquals("grad in getWorldQuadDistanceAndGradient:", gradW, gchk, grad.norm() * 1e-12);
            gchk.transform(TGL.R, grad);
            checkEquals("grad in getQuadDistanceAndGradient(T):", gradT, gchk, grad.norm() * 1e-12);
            gchk.transform(TGW.R, grad);
            checkEquals("grad in getWorldQuadDistanceAndGradient(T):", gradWT, gchk, grad.norm() * 1e-12);
            // check Dgrad
            double DgradNorm = Dgrad.frobeniusNorm();
            Matrix3d Dgchk = computeNumericQuadGradDerivative(grid, q);
            checkEquals("Dgrad in getQuadDistanceAndGradient:", Dgrad, Dgchk, DgradNorm * 5e-7);
            Dgchk.transform(TLW.R, Dgrad);
            checkEquals("Dgrad in getWorldQuadDistanceAndGradient:", DgradW, Dgchk, DgradNorm * 1e-12);
            Dgchk.transform(TGL.R, Dgrad);
            checkEquals("Dgrad in getQuadDistanceAndGradient(T):", DgradT, Dgchk, DgradNorm * 1e-12);
            Dgchk.transform(TGW.R, Dgrad);
            checkEquals("Dgrad in getWorldQuadDistanceAndGradient(T):", DgradWT, Dgchk, DgradNorm * 1e-12);
        }
    }
    testScanWrite(gridT);
}