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

Example 1 with DistanceGrid

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

the class SDGridTest method build.

@Override
public void build(String[] args) throws IOException {
    super.build(args);
    PolygonalMesh mesh = new PolygonalMesh();
    Vertex3d[] v = new Vertex3d[9 * 2 + 8];
    Face[] f = new Face[8 * 6];
    int idx = 0;
    int fidx = 0;
    double c = 64;
    v[idx++] = mesh.addVertex(-c, -c, -c);
    v[idx++] = mesh.addVertex(-c, -c, 0);
    v[idx++] = mesh.addVertex(-c, -c, c);
    v[idx++] = mesh.addVertex(-c, 0, -c);
    v[idx++] = mesh.addVertex(-c, 0, 0);
    v[idx++] = mesh.addVertex(-c, 0, c);
    v[idx++] = mesh.addVertex(-c, c, -c);
    v[idx++] = mesh.addVertex(-c, c, 0);
    v[idx++] = mesh.addVertex(-c, c, c);
    // left
    f[fidx++] = mesh.addFace(v[0], v[1], v[4]);
    f[fidx++] = mesh.addFace(v[1], v[2], v[4]);
    f[fidx++] = mesh.addFace(v[2], v[5], v[4]);
    f[fidx++] = mesh.addFace(v[5], v[8], v[4]);
    f[fidx++] = mesh.addFace(v[8], v[7], v[4]);
    f[fidx++] = mesh.addFace(v[7], v[6], v[4]);
    f[fidx++] = mesh.addFace(v[6], v[3], v[4]);
    f[fidx++] = mesh.addFace(v[3], v[0], v[4]);
    v[idx++] = mesh.addVertex(0, -c, -c);
    v[idx++] = mesh.addVertex(0, -c, 0);
    v[idx++] = mesh.addVertex(0, -c, c);
    v[idx++] = mesh.addVertex(0, 0, -c);
    // test.addVertex( 0, 0, 0);
    v[idx++] = mesh.addVertex(0, 0, c);
    v[idx++] = mesh.addVertex(0, c, -c);
    v[idx++] = mesh.addVertex(0, c, 0);
    v[idx++] = mesh.addVertex(0, c, c);
    // back
    f[fidx++] = mesh.addFace(v[0], v[9], v[10]);
    f[fidx++] = mesh.addFace(v[1], v[0], v[10]);
    f[fidx++] = mesh.addFace(v[2], v[1], v[10]);
    f[fidx++] = mesh.addFace(v[11], v[2], v[10]);
    // top
    f[fidx++] = mesh.addFace(v[2], v[11], v[13]);
    f[fidx++] = mesh.addFace(v[5], v[2], v[13]);
    f[fidx++] = mesh.addFace(v[8], v[5], v[13]);
    f[fidx++] = mesh.addFace(v[16], v[8], v[13]);
    // front
    f[fidx++] = mesh.addFace(v[8], v[16], v[15]);
    f[fidx++] = mesh.addFace(v[7], v[8], v[15]);
    f[fidx++] = mesh.addFace(v[6], v[7], v[15]);
    f[fidx++] = mesh.addFace(v[14], v[6], v[15]);
    // bottom
    f[fidx++] = mesh.addFace(v[3], v[6], v[12]);
    f[fidx++] = mesh.addFace(v[6], v[14], v[12]);
    f[fidx++] = mesh.addFace(v[0], v[3], v[12]);
    f[fidx++] = mesh.addFace(v[9], v[0], v[12]);
    v[idx++] = mesh.addVertex(c, -c, -c);
    v[idx++] = mesh.addVertex(c, -c, 0);
    v[idx++] = mesh.addVertex(c, -c, c);
    v[idx++] = mesh.addVertex(c, 0, -c);
    v[idx++] = mesh.addVertex(c, 0, 0);
    v[idx++] = mesh.addVertex(c, 0, c);
    v[idx++] = mesh.addVertex(c, c, -c);
    v[idx++] = mesh.addVertex(c, c, 0);
    v[idx++] = mesh.addVertex(c, c, c);
    // bottom
    f[fidx++] = mesh.addFace(v[17], v[9], v[12]);
    f[fidx++] = mesh.addFace(v[20], v[17], v[12]);
    f[fidx++] = mesh.addFace(v[23], v[20], v[12]);
    f[fidx++] = mesh.addFace(v[14], v[23], v[12]);
    f[fidx++] = mesh.addFace(v[9], v[17], v[10]);
    f[fidx++] = mesh.addFace(v[17], v[18], v[10]);
    f[fidx++] = mesh.addFace(v[18], v[19], v[10]);
    f[fidx++] = mesh.addFace(v[19], v[11], v[10]);
    f[fidx++] = mesh.addFace(v[11], v[19], v[13]);
    f[fidx++] = mesh.addFace(v[19], v[22], v[13]);
    f[fidx++] = mesh.addFace(v[22], v[25], v[13]);
    f[fidx++] = mesh.addFace(v[25], v[16], v[13]);
    f[fidx++] = mesh.addFace(v[16], v[25], v[15]);
    f[fidx++] = mesh.addFace(v[25], v[24], v[15]);
    f[fidx++] = mesh.addFace(v[24], v[23], v[15]);
    f[fidx++] = mesh.addFace(v[23], v[14], v[15]);
    f[fidx++] = mesh.addFace(v[25], v[22], v[21]);
    f[fidx++] = mesh.addFace(v[22], v[19], v[21]);
    f[fidx++] = mesh.addFace(v[19], v[18], v[21]);
    f[fidx++] = mesh.addFace(v[18], v[17], v[21]);
    f[fidx++] = mesh.addFace(v[17], v[20], v[21]);
    f[fidx++] = mesh.addFace(v[20], v[23], v[21]);
    f[fidx++] = mesh.addFace(v[23], v[24], v[21]);
    f[fidx++] = mesh.addFace(v[24], v[25], v[21]);
    // Move in some corner(s)
    v[0].setPosition(new Point3d(0, 0, 0));
    // v[25].setPosition(new Point3d(0,0,0));
    int divisions = 3;
    mesh = MeshFactory.subdivide(mesh, divisions);
    // randomize vertex order and re-number (to test different chiralities)
    ArrayList<Vertex3d> vertices = mesh.getVertices();
    Collections.shuffle(vertices);
    for (int i = 0; i < vertices.size(); ++i) {
        vertices.get(i).setIndex(i);
    }
    FixedMeshBody fm = new FixedMeshBody("cube", mesh);
    RenderProps.setFaceStyle(fm, FaceStyle.FRONT_AND_BACK);
    addRenderable(fm);
    int cells = 1 << (divisions + 2);
    double margin = 1.0 / cells;
    cells += 2;
    sdgrid = new DistanceGrid(mesh.getFaces(), margin, cells, /*signed=*/
    true);
    // test a bunch of inside points
    Vector3d norm = new Vector3d();
    double dx = 2 * c / ((1 << (divisions + 1)));
    for (double x = -c + dx; x < c; x += dx) {
        for (double y = -c + dx; y < c; y += dx) {
            for (double z = -c + dx; z < c; z += dx) {
                double d = sdgrid.getLocalDistanceAndNormal(norm, x, y, z);
                // check bottom corner
                if ((x < 0 && y < 0 && z < 0)) {
                    // ||(x > 0 && y > 0 && z > 0)) {
                    if (d < 0) {
                        System.err.println("Point (" + x + "," + y + "," + z + ") incorrectly labelled as inside");
                    }
                } else {
                    if (d > 0) {
                        System.err.println("Point (" + x + "," + y + "," + z + ") incorrectly labelled as outside");
                    }
                }
            }
        }
    }
    RenderProps.setDrawEdges(fm, true);
}

Example 2 with DistanceGrid

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

the class DistanceGridTest method testScanWrite.

private void testScanWrite(DistanceGrid grid) {
    StringWriter sw = new StringWriter();
    IndentingPrintWriter pw = new IndentingPrintWriter(sw);
    DistanceGrid newGrid = null;
    try {
        grid.write(pw, new NumberFormat("%g"), null);
        newGrid = new DistanceGrid();
        newGrid.scan(new ReaderTokenizer(new StringReader(sw.toString())), null);
    } catch (Exception e) {
        e.printStackTrace();
        throw new TestException("exception during write/scan test");
    }
    if (!grid.epsilonEquals(newGrid, 0)) {
        throw new TestException("write/scan test failed");
    }
}

Example 3 with DistanceGrid

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

the class DistanceGridTest method timing.

public void timing() {
    double a = 3.0;
    double b = 2.0;
    double c = 1.0;
    PolygonalMesh ellipsoid = MeshFactory.createEllipsoid(a, b, c, 48);
    Vector3i resolution = new Vector3i(20, 20, 20);
    RigidTransform3d TEW = new RigidTransform3d();
    TEW.setRandom();
    DistanceGrid grid = new DistanceGrid(ellipsoid.getFaces(), 0.1, resolution, /*signed=*/
    true);
    grid.setLocalToWorld(TEW);
    // create a bunch of random points just inside the ellispoid, in
    // both local and world coordinates
    int npnts = 10000;
    int cnt = 3000000;
    Point3d[] localPnts = new Point3d[npnts];
    Point3d[] worldPnts = new Point3d[npnts];
    for (int i = 0; i < npnts; i++) {
        Point3d plocal = new Point3d();
        double u = RandomGenerator.nextDouble(-Math.PI, Math.PI);
        double v = RandomGenerator.nextDouble(0, Math.PI);
        plocal.x = 0.9 * a * Math.cos(u) * Math.sin(v);
        plocal.y = 0.9 * b * Math.sin(u) * Math.sin(v);
        plocal.z = 0.9 * c * Math.cos(u);
        localPnts[i] = plocal;
        Point3d pworld = new Point3d();
        pworld.transform(TEW, plocal);
        worldPnts[i] = pworld;
    }
    Vector3d grad = new Vector3d();
    Matrix3d dgrad = new Matrix3d();
    // warm up the simulation
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getWorldQuadDistanceAndGradient(grad, dgrad, worldPnts[k]);
        grid.getWorldQuadDistance(worldPnts[k]);
        grid.getQuadDistanceAndGradient(grad, dgrad, localPnts[k]);
        grid.getQuadDistance(localPnts[k]);
    }
    FunctionTimer timer = new FunctionTimer();
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        Vector3d coords = new Vector3d();
        Vector3i vidx = new Vector3i();
        // grid.getQuadCellCoords (vidx, coords, localPnts[k]);
        vidx.set(2, 2, 2);
        coords.set(0.5, 0.5, 0.5);
        double dx = coords.x;
        double dy = coords.y;
        double dz = coords.z;
        int ncx = (grid.myNx - 1) / 2;
        int ncy = (grid.myNy - 1) / 2;
        int ncz = (grid.myNz - 1) / 2;
        TetID tetId = TetID.findSubTet(dx, dy, dz);
        int cidx = 6 * (vidx.x / 2 + ncx * vidx.y / 2 + ncx * ncy * vidx.z / 2) + tetId.intValue();
    // double[] aa = grid.myQuadCoefs[cidx];
    // grid.computeQuadDistance (aa, dx, dy, dz);
    }
    timer.stop();
    System.out.println("quad cell test2:            " + timer.result(cnt));
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getQuadDistance(localPnts[k]);
    }
    timer.stop();
    System.out.println("local quad dist:            " + timer.result(cnt));
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getQuadDistanceAndGradient(grad, null, localPnts[k]);
    }
    timer.stop();
    System.out.println("local quad dist/grad:       " + timer.result(cnt));
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getQuadDistanceAndGradient(grad, dgrad, localPnts[k]);
    }
    timer.stop();
    System.out.println("local quad dist/grad/dgrad: " + timer.result(cnt));
    System.out.println("");
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getWorldQuadDistance(worldPnts[k]);
    }
    timer.stop();
    System.out.println("world quad dist:            " + timer.result(cnt));
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getWorldQuadDistanceAndGradient(grad, null, worldPnts[k]);
    }
    timer.stop();
    System.out.println("world quad dist/grad:       " + timer.result(cnt));
    timer.start();
    for (int i = 0; i < cnt; i++) {
        int k = (i % npnts);
        grid.getWorldQuadDistanceAndGradient(grad, dgrad, worldPnts[k]);
    }
    timer.stop();
    System.out.println("world quad dist/grad/dgrad: " + timer.result(cnt));
}

Example 4 with DistanceGrid

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

the class MFreeFactory method generatePointLocations.

public static Point3d[] generatePointLocations(PolygonalMesh mesh, int[] res, int nPoints) {
    Vector3d centroid = new Vector3d();
    mesh.computeCentroid(centroid);
    RigidTransform3d trans = new RigidTransform3d();
    trans.setTranslation(centroid);
    OBB obb = PointDistributor.getTightOBB(mesh, trans);
    Vector3d widths = new Vector3d();
    obb.getWidths(widths);
    obb.getTransform(trans);
    int nx = res[0];
    int ny = res[1];
    int nz = res[2];
    double dx, dy, dz;
    double xOffset, yOffset, zOffset;
    if (nx == 1) {
        widths.x = 0;
        dx = 0;
    } else {
        dx = widths.x / (nx - 1);
    }
    if (ny == 1) {
        widths.y = 0;
        dy = 0;
    } else {
        dy = widths.y / (ny - 1);
    }
    if (nz == 1) {
        widths.z = 0;
        dz = 0;
    } else {
        dz = widths.z / (nz - 1);
    }
    xOffset = -widths.x / 2;
    yOffset = -widths.y / 2;
    zOffset = -widths.z / 2;
    // generate a grid of points that fall inside domain
    Point3d[][][] pnts = new Point3d[nx][ny][nz];
    // BVFeatureQuery query = new BVFeatureQuery();
    Vector3i sdres = new Vector3i(2 * nx, 2 * ny, 2 * nz);
    sdres.x = Math.min(sdres.x, 30);
    sdres.y = Math.min(sdres.y, 30);
    sdres.z = Math.min(sdres.z, 30);
    Logger.getSystemLogger().debug("Creating signed distance grid");
    List<Face> faces = mesh.getFaces();
    DistanceGrid sdgrid = new DistanceGrid(faces, 0.1, sdres, true);
    Logger.getSystemLogger().debug("done");
    double tol = 1e-15;
    Logger.getSystemLogger().debug("Generating " + nx + "x" + ny + "x" + nz + " points");
    double x, y, z;
    for (int i = 0; i < nx; i++) {
        x = xOffset + i * dx;
        for (int j = 0; j < ny; j++) {
            y = yOffset + j * dy;
            for (int k = 0; k < nz; k++) {
                z = zOffset + k * dz;
                Point3d pnt = new Point3d(x, y, z);
                pnt.transform(trans);
                double d = sdgrid.getLocalDistanceAndNormal(null, null, pnt);
                if (d < tol) {
                    pnts[i][j][k] = pnt;
                } else {
                    pnts[i][j][k] = null;
                }
            // InsideQuery rayTest = query.isInsideMesh(mesh, pnt, tol);
            // if (rayTest == InsideQuery.INSIDE) {
            // pnts[i][j][k] = pnt;
            // } else if (rayTest == InsideQuery.OUTSIDE) {
            // pnts[i][j][k] = null;
            // } else {
            // System.out.println("unsure");
            // }
            }
        }
    }
    Logger.getSystemLogger().debug("done.");
    Logger.getSystemLogger().debug("Farthest point sampling...");
    Point3dGridUtility pgu = new Point3dGridUtility(pnts);
    FastRadialMarcher marcher = new FastRadialMarcher(nx * ny * nz, pgu);
    marcher.initializeArrays();
    double[] dists = marcher.getDistance();
    // mark null points as having distance of -1 (never to be selected)
    for (int i = 0; i < nx; i++) {
        for (int j = 0; j < ny; j++) {
            for (int k = 0; k < nz; k++) {
                if (pnts[i][j][k] == null) {
                    dists[k + j * nz + i * ny * nz] = -1;
                }
            }
        }
    }
    // initialize heap to include all indices
    marcher.getDistanceHeap().setAll();
    // farthest-point sampling
    Point3d[] out = new Point3d[nPoints];
    int i, j, k;
    for (int idx = 0; idx < nPoints; idx++) {
        int farthest = 0;
        IndexedBinaryHeap dheap = marcher.getDistanceHeap();
        farthest = dheap.peek();
        // get next furthest
        int nextSample = farthest;
        // idx = i*ny*nz+j*nz+k
        k = nextSample % nz;
        j = ((nextSample - k) / nz) % ny;
        i = (nextSample - k - j * nz) / (ny * nz);
        out[idx] = pnts[i][j][k];
        // update distances to first point
        marcher.march(nextSample);
    }
    Logger.getSystemLogger().debug("done.");
    return out;
}

Example 5 with DistanceGrid

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

the class RigidBody method getDistanceSurface.

protected PolygonalMesh getDistanceSurface() {
    if (mySDSurface == null) {
        if (hasDistanceGrid()) {
            PolygonalMesh surf;
            DistanceGrid grid = getDistanceGrid();
            if (myRenderDistanceSurface == DistanceSurfaceRendering.QUADRATIC) {
                surf = grid.createQuadDistanceSurface(myDistanceSurfaceIso, 4);
                surf.setMeshToWorld(myState.XFrameToWorld);
            } else {
                surf = grid.createDistanceSurface(myDistanceSurfaceIso, 4);
                surf.setMeshToWorld(myState.XFrameToWorld);
            }
            mySDSurface = surf;
        }
    }
    return mySDSurface;
}