Examples with Vector3i maspack.matrix.Vector3i used on opensource projects

Example 11 with Vector3i

use of maspack.matrix.Vector3i in project artisynth_core by artisynth.

the class DistanceGridSurfCalc method createConnectivity.

private void createConnectivity() {
    myFaceTets = new HashMap<TetFace, TetDesc>();
    ArrayList<HashSet<TetDesc>> nodeTets = new ArrayList<HashSet<TetDesc>>();
    for (int i = 0; i < 8; i++) {
        nodeTets.add(new HashSet<TetDesc>());
    }
    HashMap<TetEdge, HashSet<TetDesc>> edgeTets = new HashMap<TetEdge, HashSet<TetDesc>>();
    for (int i = -1; i <= 1; i++) {
        for (int j = -1; j <= 1; j++) {
            for (int k = -1; k <= 1; k++) {
                for (TetID tetId : TetID.values()) {
                    TetDesc tdesc = new TetDesc(new Vector3i(i, j, k), tetId);
                    Vector3i[] verts = tdesc.getVertices();
                    for (int vi = 0; vi < verts.length; vi++) {
                        int refVtx = findRefVertex(verts[vi]);
                        if (refVtx != -1) {
                            nodeTets.get(refVtx).add(tdesc);
                        }
                    }
                    maybeAddFace(myFaceTets, verts[0], verts[1], verts[2], tdesc);
                    maybeAddFace(myFaceTets, verts[0], verts[2], verts[3], tdesc);
                    maybeAddFace(myFaceTets, verts[2], verts[1], verts[3], tdesc);
                    maybeAddFace(myFaceTets, verts[1], verts[0], verts[3], tdesc);
                    maybeAddEdge(edgeTets, verts[0], verts[1], tdesc);
                    maybeAddEdge(edgeTets, verts[1], verts[2], tdesc);
                    maybeAddEdge(edgeTets, verts[2], verts[0], tdesc);
                    maybeAddEdge(edgeTets, verts[0], verts[3], tdesc);
                    maybeAddEdge(edgeTets, verts[1], verts[3], tdesc);
                    maybeAddEdge(edgeTets, verts[2], verts[3], tdesc);
                }
            }
        }
    }
    myNodeTets = new ArrayList<TetDesc[]>();
    for (int i = 0; i < nodeTets.size(); i++) {
        myNodeTets.add(nodeTets.get(i).toArray(new TetDesc[0]));
    }
    myEdgeTets = new HashMap<TetEdge, TetDesc[]>();
    for (TetEdge edge : edgeTets.keySet()) {
        myEdgeTets.put(edge, edgeTets.get(edge).toArray(new TetDesc[0]));
    }
}

Example 12 with Vector3i

use of maspack.matrix.Vector3i in project artisynth_core by artisynth.

the class DistanceGridSurfCalc method findQuadSurfaceIntersectionLoc.

/**
 * Find the nearest quad surface intersection to p0, in the direction dir,
 * and return the result in ps. It is assumed that dir lies in the
 * plane. Input and output parameters are all given in grid local
 * coordinates. The method returns a descriptor of the tet/plane
 * intersection for the tet containing ps, unless ps is not found, in which
 * case null is returned.
 */
public TetDesc findQuadSurfaceIntersectionLoc(Point3d ps, Point3d p0, Vector3d dir) {
    TetDesc tdesc = null;
    // find the quadratic cell containing p0
    Vector3i vxyz = new Vector3i();
    Vector3d xyz = new Vector3d();
    if (myGrid.getQuadCellCoords(xyz, vxyz, p0, myGrid.myQuadGridToLocal) == -1) {
        // shouldn't happen - grid should have enough margin to prevent this
        if (myDebug > 0) {
            System.out.println("Not found: no quad cell found for p0");
        }
        return null;
    }
    // find the tet containing p0 within the quadratic cell
    tdesc = new TetDesc(vxyz, TetID.findSubTet(xyz.x, xyz.y, xyz.z));
    // now transform p0 and dir to quad grid coordinates
    Point3d p0Quad = new Point3d();
    Vector3d dirQuad = new Vector3d();
    myGrid.myQuadGridToLocal.inverseTransformPnt(p0Quad, p0);
    myGrid.myQuadGridToLocal.inverseTransformVec(dirQuad, dir);
    if (myDebug > 0) {
        System.out.println("looking for ps starting from " + tdesc);
    }
    // Find the interval (srng[0], srng[1]) within which the ray (p0, dir)
    // intersects tet/plane intersection defined by isect. This interval is
    // used to seed the surface intersection search. The edge of the
    // intersection boundary which clips the upper value of the range is
    // returned in edgeIdx, and the intersection parameter along that edge is
    // placed in edgeS.
    double[] srng = new double[] { 0, Double.MAX_VALUE };
    tdesc.clipLineSegment(srng, p0Quad, dirQuad);
    if (srng[0] > srng[1]) {
        // shouldn't happen - p0 should be in the tet
        if (myDebug > 0) {
            System.out.println("Not found: p0 tet does not intersect ray (p0,dir)");
        }
        return null;
    }
    // Find the tet boundary feature associated with the upper bound of the
    // ray intersection. This will be used to search for adjacent tets in
    // case the surface intersection does not occur within the current tet.
    Point3d px = new Point3d();
    px.scaledAdd(srng[1], dir, p0);
    // TetFeature lastFeat = isect.getFeature (edgeS.value, edgeIdx);
    TetFeature lastFeat = findNearestFeature(tdesc, px, 1e-10);
    myVisitedTets.clear();
    myVisitedTets.add(tdesc);
    // first point ps at which (p0, dir) intersects the quadratic surface.
    while (findSurfaceIntersectionInTet(ps, tdesc, srng, p0Quad, dirQuad) == CONTINUE) {
        // A return value of CONTINUE means that the surface intersection was
        // not found in the current tet, and that we should look for the
        // intersection in adjacent tets.
        ArrayList<TetDesc> adescs = new ArrayList<TetDesc>();
        int ntets = getFeatureAdjacentTets(adescs, tdesc, lastFeat, myVisitedTets);
        TetDesc tbest = null;
        double bestLen = 0;
        if (myDebug > 0) {
            System.out.println("checking " + ntets + " adjacent tets for feature " + lastFeat);
            System.out.print("  ");
            for (int k = 0; k < ntets; k++) {
                System.out.print(adescs.get(k) + " ");
            }
            System.out.println("");
        }
        // intersection intersects the ray over the longest length.
        for (int k = 0; k < ntets; k++) {
            TetDesc adesc = adescs.get(k);
            double[] irng = new double[] { 0, Double.MAX_VALUE };
            adesc.clipLineSegment(irng, p0Quad, dirQuad);
            double ilen = irng[1] - irng[0];
            if (myDebug > 0) {
                System.out.println("    ilen=" + ilen);
            }
            if (ilen > bestLen) {
                bestLen = ilen;
                tbest = adesc;
                srng[0] = irng[0];
                srng[1] = irng[1];
            }
        }
        if (tbest == null) {
            if (myDebug > 0) {
                System.out.println("Not found: no adjacent tets intersect the ray");
            }
            return null;
        } else {
            px.scaledAdd(srng[1], dir, p0);
            // lastFeat = ibest.getFeature (bestS, bestEdgeIdx);
            lastFeat = findNearestFeature(tbest, px, 1e-10);
            tdesc = tbest;
            myVisitedTets.add(tdesc);
        }
    }
    myGrid.myQuadGridToLocal.transformPnt(ps, ps);
    return tdesc;
}

Example 13 with Vector3i

use of maspack.matrix.Vector3i in project artisynth_core by artisynth.

the class DistanceGridTester method checkSignOfPhi.

// /**
// * This test is a basic verification of SignedDistanceGrid.gridCellArray[].
// * We're making sure that it matches phi[]. This is a trivial but important
// * assurance because the visual inspection tests rely on gridCellArray to
// * match phi.
// */
// public void checkCellArray () {
// // go through all of phi and compare with gridCellArray.
// double myPhi[] = g.getDistances ();
// for (int i = 0; i < myPhi.length; i++) {
// if (myPhi[i] != this.g.getGridCell(i).getDistance()) {
// throw new TestException("cellArray distance does not match phi");
// }
// }
// System.out.println ("Cell Array matches Phi, Test passed.");
// }
/**
 * This test checks the sign of each point in a rectangular grid. We
 * manually check that every point inside the mesh boundaries has a negative
 * distance, and every point outside has a positive distance.
 * Warning: this method only works for rectangular prisms.
 */
private void checkSignOfPhi() {
    double[] myPhi = g.getDistances();
    Vector3d min = new Vector3d();
    Vector3d max = new Vector3d();
    m.getLocalBounds(min, max);
    for (int i = 0; i < myPhi.length; i++) {
        // int z = i / (gridSize[0] * gridSize[1]);
        // int y = (i - z * gridSize[0] * gridSize[1]) / gridSize[0];
        // int x = i % gridSize[0];
        // translate to mesh coordinates.
        Vector3d coords = new Vector3d();
        g.getLocalVertexCoords(coords, g.vertexToXyzIndices(new Vector3i(), i));
        // If our point lies inside all the boundaries
        if ((coords.x > min.x && coords.x < max.x) && (coords.y > min.y && coords.y < max.y) && (coords.z > min.z && coords.z < max.z)) {
            if (myPhi[i] > 0) {
                // myPhi should be < 0
                throw new TestException("Phi is positive when it should be negative");
            }
        } else if ((coords.x < min.x || coords.x > max.x) || (coords.y < min.y || coords.y > max.y) || (coords.z < min.z || coords.z > max.z)) {
            if (myPhi[i] < 0) {
                // myPhi should be > 0
                throw new TestException("Phi is negative when it should be positive");
            }
        }
    }
}

Example 14 with Vector3i

use of maspack.matrix.Vector3i 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 15 with Vector3i

use of maspack.matrix.Vector3i in project artisynth_core by artisynth.

the class RigidBody method getDistanceGrid.

/**
 * Returns a signed distance grid that can be used with a
 * SignedDistanceCollider, or <code>null</code> if a grid is not available
 * (i.e., if {@link #hasDistanceGrid} returns <code>false</code>).
 * The number of divisons in the grid is controlled explicitly by the
 * property <code>distanceGridRes</code>, or, that is 0, by the property
 * <code>distanceGridMaxRes</code>. If both properties are 0, no grid is
 * available and <code>null</code> will be returned.
 *
 * @return signed distance grid, or <code>null</code> if a grid is
 * not available this RigidBody
 * @see #getDistanceGridMaxRes
 * @see #getDistanceGridRes
 */
public DistanceGrid getDistanceGrid() {
    if (mySDGrid == null || !mySDGridValid) {
        int maxRes = myDistanceGridMaxRes;
        if (!myDistanceGridRes.equals(Vector3i.ZERO) || maxRes > 0) {
            if (!myDistanceGridRes.equals(Vector3i.ZERO)) {
                mySDGrid = new DistanceGrid(myDistanceGridRes);
                maxRes = 0;
            } else {
                // resolution will be recomputed in computeFromFeatures
                mySDGrid = new DistanceGrid(new Vector3i(1, 1, 1));
            }
            mySDGrid.setDrawEdges(true);
            List<Face> faces = getMesh().getFaces();
            if (myDistanceGridOBB) {
                mySDGrid.computeFromFeaturesOBB(faces, myGridMargin, maxRes, /*signed=*/
                true);
            } else {
                mySDGrid.computeFromFeatures(faces, myGridMargin, /*TGL=*/
                null, maxRes, /*signed=*/
                true);
            }
            mySDGrid.setLocalToWorld(getPose());
        } else {
            mySDGrid = null;
        }
        // mySDGrid.smooth (0.33, -0.34, 2);
        mySDSurface = null;
        mySDGridValid = true;
    }
    return mySDGrid;
}