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

Example 16 with Vector3d

use of maspack.matrix.Vector3d 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 17 with Vector3d

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

the class DistanceGrid method scan.

public void scan(ReaderTokenizer rtok, Object ref) throws IOException {
    rtok.scanToken('[');
    RigidTransform3d TCL = new RigidTransform3d();
    RigidTransform3d TLW = null;
    Vector3d widths = new Vector3d();
    Vector3i resolution = null;
    myTLocalToWorld = null;
    while (rtok.nextToken() != ']') {
        if (rtok.ttype != ReaderTokenizer.TT_WORD) {
            throw new IOException("Expected attribute name, " + rtok);
        }
        if (rtok.sval.equals("resolution")) {
            rtok.scanToken('=');
            resolution = new Vector3i();
            resolution.scan(rtok);
            setResolution(resolution);
        } else if (rtok.sval.equals("widths")) {
            rtok.scanToken('=');
            widths.scan(rtok);
            setWidths(widths);
        } else if (rtok.sval.equals("center")) {
            rtok.scanToken('=');
            TCL.p.scan(rtok);
        } else if (rtok.sval.equals("orientation")) {
            rtok.scanToken('=');
            TCL.R.scan(rtok);
        } else if (rtok.sval.equals("LocalToWorld")) {
            rtok.scanToken('=');
            TLW = new RigidTransform3d();
            TLW.scan(rtok);
        } else if (rtok.sval.equals("distances")) {
            if (resolution == null) {
                throw new IOException("distances must be specified after resolution, line " + rtok.lineno());
            }
            rtok.scanToken('=');
            rtok.scanToken('[');
            int k = 0;
            while (rtok.nextToken() != ']') {
                rtok.pushBack();
                myPhi[k++] = rtok.scanNumber();
            }
        } else {
            throw new IOException("Unexpected attribute name: " + rtok);
        }
    }
    setCenterAndOrientation(TCL);
    if (TLW != null) {
        setLocalToWorld(TLW);
    }
    // clear normals and quad coefs; shouldn't be necessary if resolution
    // was set
    myQuadCoefs = null;
    clearNormals();
}

Example 18 with Vector3d

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

the class DistanceGrid method createDistanceSurface.

/**
 * Creates a triangular mesh approximating the surface on which the linearly
 * interpolated distance function equals <code>val</code>.
 *
 * @param val iso surface value
 * @return iso surface for linear interpolation
 */
public PolygonalMesh createDistanceSurface(double val) {
    MarchingTetrahedra marcher = new MarchingTetrahedra();
    PolygonalMesh mesh = marcher.createMesh(myPhi, Vector3d.ZERO, new Vector3d(1, 1, 1), getResolution(), val);
    mesh.transform(myGridToLocal);
    return mesh;
}

Example 19 with Vector3d

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

the class DistanceGrid method fitOBB.

private void fitOBB(Vector3d widths, RigidTransform3d TCL, double marginFrac, Collection<List<? extends Feature>> featureSets, OBB.Method method) {
    int nfeats = 0;
    for (List<? extends Feature> features : featureSets) {
        nfeats += features.size();
    }
    Feature[] allFeats = new Feature[nfeats];
    int k = 0;
    for (List<? extends Feature> features : featureSets) {
        for (Feature f : features) {
            allFeats[k++] = f;
        }
    }
    OBB obb = new OBB();
    obb.set(allFeats, nfeats, 0, method);
    Vector3d halfWidths = new Vector3d();
    obb.getHalfWidths(halfWidths);
    widths.scale(2, halfWidths);
    adjustWidths(widths, marginFrac);
    obb.getTransform(TCL);
}

Example 20 with Vector3d

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

the class DistanceGrid method doGetQuadDistanceAndGradient.

protected double doGetQuadDistanceAndGradient(Vector3d grad, Matrix3d dgrad, Point3d point, VectorTransformer3d quadGridToX) {
    Vector3d coords = new Vector3d();
    double[] a;
    double dx, dy, dz;
    if (storeQuadCoefs) {
        updateQuadCoefsIfNecessary();
        int voff = getQuadCellCoords(coords, null, point, quadGridToX);
        if (voff == -1) {
            return OUTSIDE_GRID;
        }
        dx = coords.x;
        dy = coords.y;
        dz = coords.z;
        a = myQuadCoefs[6 * voff + TetID.findSubTetIdx(dx, dy, dz)];
    } else {
        // Change to grid coordinates
        Vector3i vidx = new Vector3i();
        if (getQuadCellCoords(coords, vidx, point, quadGridToX) == -1) {
            return OUTSIDE_GRID;
        }
        dx = coords.x;
        dy = coords.y;
        dz = coords.z;
        a = new double[10];
        TetDesc tdesc = new TetDesc(vidx, TetID.findSubTet(dx, dy, dz));
        computeQuadCoefs(a, tdesc);
    }
    if (grad != null) {
        computeQuadGradient(grad, a, dx, dy, dz, quadGridToX);
    }
    if (dgrad != null) {
        computeQuadHessian(dgrad, a, quadGridToX);
    }
    return computeQuadDistance(a, dx, dy, dz);
}