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

Example 41 with Vector3d

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

the class DistanceGridSurfCalc method findNearestFeature.

/**
 * Find the nearest feature on a tet to a point p0. eps is a tolerance
 * (in the range [0,1]) used to check the barycentric coordinates
 * of p0 to see if it is actually close to a feature. If it
 * is not, this method returns null.
 */
TetFeature findNearestFeature(TetDesc desc, Point3d p0Loc, double eps) {
    if (myTetBarycentricMats == null) {
        createTetBarycentricMats();
    }
    // convert p0Loc to quad grid coordinates, and the multiply by the
    // barycentric conversion matrix to get barycentric coordinates s1, s2,
    // s3. s0 is then given by s0 = 1 - s1 - s2 - s2.
    Point3d pc = new Point3d();
    Vector3d sv = new Vector3d();
    transformToQuadCell(pc, p0Loc, desc);
    myTetBarycentricMats[desc.myTetId.intValue()].mul(sv, pc);
    // code is a bit code describing which coordinates are close to 0
    int code = 0;
    if (Math.abs(1 - sv.get(0) - sv.get(1) - sv.get(2)) < eps) {
        code |= 0x01;
    }
    if (Math.abs(sv.get(0)) < eps) {
        code |= 0x02;
    }
    if (Math.abs(sv.get(1)) < eps) {
        code |= 0x04;
    }
    if (Math.abs(sv.get(2)) < eps) {
        code |= 0x08;
    }
    int[] nodes = desc.myTetId.getNodes();
    switch(code) {
        case 0x01:
            {
                return new TetFace(nodes[1], nodes[3], nodes[2]);
            }
        case 0x02:
            {
                return new TetFace(nodes[0], nodes[2], nodes[3]);
            }
        case 0x04:
            {
                return new TetFace(nodes[0], nodes[3], nodes[1]);
            }
        case 0x08:
            {
                return new TetFace(nodes[0], nodes[1], nodes[2]);
            }
        case 0x03:
            return new TetEdge(nodes[2], nodes[3]);
        case 0x05:
            return new TetEdge(nodes[1], nodes[3]);
        case 0x09:
            return new TetEdge(nodes[1], nodes[2]);
        case 0x06:
            return new TetEdge(nodes[0], nodes[3]);
        case 0x0a:
            return new TetEdge(nodes[0], nodes[2]);
        case 0x0c:
            return new TetEdge(nodes[0], nodes[1]);
        case 0x0e:
            return new TetNode(nodes[0]);
        case 0x0d:
            return new TetNode(nodes[1]);
        case 0x0b:
            return new TetNode(nodes[2]);
        case 0x07:
            return new TetNode(nodes[3]);
        default:
            {
                return null;
            }
    }
}

Example 42 with Vector3d

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

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

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

the class Face method nearestPointFace.

private void nearestPointFace(Point3d pc, Point3d p1) {
    // long time = System.nanoTime();
    HalfEdge he = he0;
    Vector3d dv = new Vector3d();
    if (myNormal == null) {
        myNormal = new Vector3d();
    }
    computeNormal(myNormal);
    do {
        HalfEdge heNext = he.next;
        dv.sub(p1, he.head.pnt);
        // /
        double dotNext = heNext.dotDirection(dv);
        // /
        double dotPrev = he.dotDirection(dv);
        if (dotNext <= 0 && dotPrev >= 0) {
            // then the closest point is he.head
            pc.set(he.head.pnt);
            // time;
            return;
        }
        double lenNextSqr = heNext.lengthSquared();
        if (dotNext > 0 && dotNext < lenNextSqr && heNext.sideProductDirection(dv, myNormal) >= 0) {
            // then the closest point is on the edge heNext
            dv.sub(heNext.head.pnt, heNext.tail.pnt);
            pc.scaledAdd(dotNext / lenNextSqr, dv, heNext.tail.pnt);
            // time;
            return;
        }
        he = heNext;
    } while (he != he0);
    // the closest point is on the face
    dv.sub(p1, he0.head.pnt);
    double d = dv.dot(myNormal);
    pc.scaledAdd(-d, myNormal, p1);
    // time;
    return;
}

Example 45 with Vector3d

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

the class Face method maxCosine.

/**
 * Returns the maximum cosine of the triangle formed from a set of three
 * vertices.
 */
private double maxCosine(Vertex3d vtx0, Vertex3d vtx1, Vertex3d vtx2) {
    Vector3d u01 = new Vector3d();
    Vector3d u12 = new Vector3d();
    Vector3d u20 = new Vector3d();
    u01.sub(vtx1.pnt, vtx0.pnt);
    u01.normalize();
    u12.sub(vtx2.pnt, vtx1.pnt);
    u12.normalize();
    u20.sub(vtx0.pnt, vtx2.pnt);
    u20.normalize();
    double maxCos = u20.dot(u01);
    double c = u01.dot(u12);
    if (c > maxCos) {
        maxCos = c;
    }
    c = u12.dot(u20);
    if (c > maxCos) {
        maxCos = c;
    }
    return maxCos;
}