Examples with InternalErrorException maspack.util.InternalErrorException used on opensource projects

Example 1 with InternalErrorException

use of maspack.util.InternalErrorException in project artisynth_core by artisynth.

the class DistanceGrid method calculatePhi.

/**
 * Calculates the distance field.
 */
void calculatePhi(double[] phi, int[] closestFeatureIdxs, Feature[] features, boolean signed) {
    // Logger logger = Logger.getSystemLogger();
    // logger.info ("Calculating Distance Field...");
    double maxDist = 2 * getRadius();
    int numv = myNx * myNy * myNz;
    if (phi.length != numv || (closestFeatureIdxs != null && closestFeatureIdxs.length != numv)) {
        throw new InternalErrorException("length of phi and/or closestFeatureIdxs != numVertices()");
    }
    for (int k = 0; k < numv; k++) {
        phi[k] = maxDist;
    }
    // Feature to the grid vertex.
    if (closestFeatureIdxs != null) {
        for (int i = 0; i < numv; i++) {
            closestFeatureIdxs[i] = -1;
        }
    }
    int[] zIntersectCount = null;
    if (signed) {
        zIntersectCount = new int[numv];
        for (int i = 0; i < numv; i++) {
            zIntersectCount[i] = 0;
        }
    }
    Point3d featPnt = new Point3d();
    Point3d gridPnt = new Point3d();
    Vector3i gridMinOld = new Vector3i();
    Vector3i gridMaxOld = new Vector3i();
    Vector3i gridMin = new Vector3i();
    Vector3i gridMax = new Vector3i();
    Point3d featMin = new Point3d();
    Point3d featMax = new Point3d();
    Point3d nearPntLoc = new Point3d();
    Point3d featPntLoc = new Point3d();
    Vector3i hi = new Vector3i();
    Vector3i lo = new Vector3i();
    // For every feature ...
    for (int t = 0; t < features.length; ++t) {
        // Find the vertex-aligned parallelpiped containing the feature's
        // bounding box.
        Feature feature = features[t];
        gridMin.set(myNx + 1, myNy + 1, myNz + 1);
        gridMax.set(-1, -1, -1);
        // max, minz of the feature in grid coords
        double maxz = -INF;
        double minz = INF;
        for (int i = 0; i < feature.numPoints(); i++) {
            featPnt = feature.getPoint(i);
            myGridToLocal.inverseTransformPnt(gridPnt, featPnt);
            lo.x = clip((int) gridPnt.x, 0, myNx - 1);
            lo.y = clip((int) gridPnt.y, 0, myNy - 1);
            lo.z = clip((int) gridPnt.z, 0, myNz - 1);
            hi.x = clip((int) (gridPnt.x + 1), 0, myNx - 1);
            hi.y = clip((int) (gridPnt.y + 1), 0, myNy - 1);
            hi.z = clip((int) (gridPnt.z + 1), 0, myNz - 1);
            if (gridPnt.z < minz) {
                minz = gridPnt.z;
            }
            if (gridPnt.z > maxz) {
                maxz = gridPnt.z;
            }
            lo.updateBounds(gridMin, gridMax);
            hi.updateBounds(gridMin, gridMax);
        }
        // closestFeature.
        for (int zk = gridMin.z; zk <= gridMax.z; zk++) {
            for (int yj = gridMin.y; yj <= gridMax.y; yj++) {
                for (int xi = gridMin.x; xi <= gridMax.x; xi++) {
                    // Get features coordinates
                    featPntLoc.set(xi, yj, zk);
                    myGridToLocal.transformPnt(featPntLoc, featPntLoc);
                    // Get the distance from this point to the Feature.
                    feature.nearestPoint(nearPntLoc, featPntLoc);
                    double distance = featPntLoc.distance(nearPntLoc);
                    int index = xyzIndicesToVertex(xi, yj, zk);
                    if (index >= numv) {
                        System.out.println("coords = " + xi + " " + yj + " " + zk);
                    }
                    if (distance < phi[index]) {
                        phi[index] = distance;
                        if (closestFeatureIdxs != null) {
                            closestFeatureIdxs[index] = t;
                        }
                    }
                }
            }
        }
        if (signed) {
            if (!(feature instanceof Face)) {
                throw new IllegalArgumentException("Signed grid can only be created if all features are Faces");
            }
            Face face = (Face) feature;
            Point3d bot = new Point3d();
            Point3d top = new Point3d();
            // We're building intersectionCount[] to use in ray casting below.
            for (int yj = gridMin.y; yj <= gridMax.y; yj++) {
                for (int xi = gridMin.x; xi <= gridMax.x; xi++) {
                    Point3d ipnt = new Point3d();
                    int res = 0;
                    if (maxz >= 0) {
                        myGridToLocal.transformPnt(bot, new Point3d(xi, yj, minz - 1));
                        myGridToLocal.transformPnt(top, new Point3d(xi, yj, maxz + 1));
                        res = RobustPreds.intersectSegmentTriangle(ipnt, bot, top, face, maxDist, /*worldCoords=*/
                        false);
                    }
                    if (res > 0) {
                        myGridToLocal.inverseTransformPnt(gridPnt, ipnt);
                        int zInterval = clip((int) Math.ceil(gridPnt.z), 0, myNz - 1);
                        ++zIntersectCount[xyzIndicesToVertex(xi, yj, zInterval)];
                    }
                // point in triangle
                }
            // x
            }
        // y
        }
    }
    // Sweep, propagating values throughout the grid volume.
    for (int pass = 0; pass < 2; pass++) {
        sweep(phi, +1, +1, +1, closestFeatureIdxs, features);
        sweep(phi, -1, -1, -1, closestFeatureIdxs, features);
        sweep(phi, +1, +1, -1, closestFeatureIdxs, features);
        sweep(phi, -1, -1, +1, closestFeatureIdxs, features);
        sweep(phi, +1, -1, +1, closestFeatureIdxs, features);
        sweep(phi, -1, +1, -1, closestFeatureIdxs, features);
        sweep(phi, +1, -1, -1, closestFeatureIdxs, features);
        sweep(phi, -1, +1, +1, closestFeatureIdxs, features);
    }
    if (signed) {
        // vertex in the grid.
        for (int xi = 0; xi < myNx; xi++) {
            for (int yj = 0; yj < myNy; yj++) {
                int total_count = 0;
                // Count the intersections of the x axis
                for (int zk = 0; zk < myNz; zk++) {
                    int index = xyzIndicesToVertex(xi, yj, zk);
                    total_count += zIntersectCount[index];
                    // mesh.
                    if (total_count % 2 == 1) {
                        phi[index] = -phi[index];
                    }
                }
            }
        }
    }
}

Example 2 with InternalErrorException

use of maspack.util.InternalErrorException in project artisynth_core by artisynth.

the class DistanceGrid method computeQuadCoefs.

public void computeQuadCoefs(double[] a, TetDesc tdesc) {
    createTetOffsetsIfNecessary();
    int voff = xyzIndicesToVertex(2 * tdesc.myCXi, 2 * tdesc.myCYj, 2 * tdesc.myCZk);
    for (int i = 0; i < 10; i++) a[i] = 0;
    switch(tdesc.myTetId) {
        case TET0516:
            {
                computeQuadCoefs(a, voff, myTetOffsets0156, 0, 1, 2);
                break;
            }
        case TET0456:
            {
                computeQuadCoefs(a, voff, myTetOffsets0456, 1, 0, 2);
                break;
            }
        case TET0746:
            {
                computeQuadCoefs(a, voff, myTetOffsets0476, 1, 2, 0);
                break;
            }
        case TET0126:
            {
                computeQuadCoefs(a, voff, myTetOffsets0126, 0, 2, 1);
                break;
            }
        case TET0236:
            {
                computeQuadCoefs(a, voff, myTetOffsets0326, 2, 0, 1);
                break;
            }
        case TET0376:
            {
                computeQuadCoefs(a, voff, myTetOffsets0376, 2, 1, 0);
                break;
            }
        default:
            {
                throw new InternalErrorException("Unknown tet type " + tdesc.myTetId);
            }
    }
}

Example 3 with InternalErrorException

use of maspack.util.InternalErrorException in project artisynth_core by artisynth.

the class MeshFactory method collectEdgeVertices.

private static Vertex3d[] collectEdgeVertices(HashMap<HalfEdge, Vertex3d[]> edgeVertices, PolygonalMesh mesh, Face face, int edgeNum, int res, NagataInterpolator interp) {
    int numv = res + 1;
    HalfEdge he = face.getEdge(edgeNum);
    Vertex3d[] vtxs = edgeVertices.get(he.opposite);
    if (vtxs == null) {
        // need to create the vertices
        vtxs = new Vertex3d[numv];
        vtxs[0] = he.getTail();
        vtxs[numv - 1] = he.getHead();
        // create vertices along edge
        for (int i = 1; i < numv - 1; i++) {
            Point3d pnt = new Point3d();
            double eta, zeta;
            double s = i / (double) res;
            switch(edgeNum) {
                case 1:
                    {
                        eta = s;
                        zeta = 0;
                        break;
                    }
                case 2:
                    {
                        eta = 1;
                        zeta = s;
                        break;
                    }
                case 0:
                    {
                        eta = 1 - s;
                        zeta = 1 - s;
                        break;
                    }
                default:
                    {
                        throw new InternalErrorException("Illegal edgeNum " + edgeNum);
                    }
            }
            interp.interpolateVertex(pnt, eta, zeta);
            // pnt.combine (0.5, vtxs[0].pnt, 0.5, vtxs[numv-1].pnt);
            vtxs[i] = new Vertex3d(pnt);
            mesh.addVertex(vtxs[i]);
        }
        edgeVertices.put(he, vtxs);
        return vtxs;
    } else {
        // vertices in the wrong order; return a reversed list of them.
        Vertex3d[] vtxsRev = new Vertex3d[numv];
        for (int i = 0; i < numv; i++) {
            vtxsRev[i] = vtxs[numv - 1 - i];
        }
        return vtxsRev;
    }
}

Example 4 with InternalErrorException

use of maspack.util.InternalErrorException in project artisynth_core by artisynth.

the class PolygonalMesh method computeVertexNormals.

/**
 * Computes a set of vertex normals for this mesh, using an
 * angle-weighted average of the normals formed by the edges incident on
 * each vertex. If the angle-weighted average would result in a zero normal
 * (e.g. vertices on a straight line), then the adjacent face normals
 * are used.  If <code>multiNormals</code> is <code>true</code>, then
 * multiple normals may be computed for each vertex, with different normals
 * being computed for edge regions that are separated by open or hard
 * edges. Otherwise, only one normal is computed per vertex.
 *
 * <p>If <code>normals</code> is passed in with zero size, then the normals
 * are computed and returned in new <code>Vector3d</code> objects that are
 * and added to it. Also, the method returns a set of computed normal
 * indices. This option is used for the initial creation of normals.
 *
 * <p>If <code>normals</code> is passed in with non-zero size, then it is
 * assumed to contain enough <code>Vector3d</code> objects to store all the
 * computed normals, and the method returns <code>null</code>.  This option
 * is used for updating normals.
 *
 * @param normals returns the computed normals
 * @param multiNormals if <code>true</code>, then multiple normals
 * may be computed for each vertex
 * @return normals indices, if <code>normals</code> has zero size,
 * otherwise <code>null</code>.
 */
public int[] computeVertexNormals(ArrayList<Vector3d> normals, boolean multiNormals) {
    boolean creatingNormals = (normals.size() == 0);
    if (multiNormals) {
        // make sure hard edges are properly set
        updateHardEdgeCount();
    }
    HashMap<HalfEdge, Integer> normalIndexMap = null;
    if (creatingNormals) {
        // Each half edge will be associated with a normal for its head vertex.
        normalIndexMap = new HashMap<HalfEdge, Integer>();
    }
    // Start by allocating normals and determining the normal index
    // associated with each half-face
    int idx = 0;
    for (Vertex3d vtx : myVertices) {
        HalfEdgeNode node = vtx.getIncidentHedges();
        Vector3d nrm;
        while (node != null) {
            if (creatingNormals) {
                // create a new vector to store the normal
                nrm = new Vector3d();
                normals.add(nrm);
            } else {
                // use the existing normal vector
                nrm = normals.get(idx);
                nrm.setZero();
            }
            // reach a normal boundary.
            do {
                HalfEdge he = node.he;
                nrm.angleWeightedCrossAdd(he.tail.pnt, he.head.pnt, he.next.head.pnt);
                if (creatingNormals) {
                    normalIndexMap.put(node.he, idx);
                }
                node = node.next;
            } while (node != null && (!multiNormals || !vtx.isNormalBoundary(node.he)));
            double n2 = nrm.normSquared();
            if (n2 == 0) {
                // backup, just in case angle weighted normals fails
                vtx.computeAreaWeightedNormal(nrm);
            // nmag = nrm.norm();
            }
            nrm.normalize();
            // if (nmag > 0) {
            // nrm.scale(1.0/nmag);
            // }
            idx++;
        }
    }
    if (creatingNormals) {
        // Now assign the normal indices for each face. These are the indices of
        // the normals associated with each of the face's half edges.
        int[] indexOffs = getFeatureIndexOffsets();
        int[] indices = new int[indexOffs[indexOffs.length - 1]];
        int k = 0;
        for (Face face : myFaces) {
            HalfEdge he0 = face.firstHalfEdge();
            HalfEdge he = he0;
            do {
                Integer id = normalIndexMap.get(he);
                if (id == null) {
                    throw new InternalErrorException("Normal not computed for halfEge on face " + face.getIndex());
                }
                indices[k++] = id;
                he = he.getNext();
            } while (he != he0);
        }
        return indices;
    } else {
        return null;
    }
}

Example 5 with InternalErrorException

use of maspack.util.InternalErrorException in project artisynth_core by artisynth.

the class OBB method set.

public void set(Boundable[] elems, int num, double margin, Method method) {
    Matrix3d C = new Matrix3d();
    Point3d cent = new Point3d();
    Point3d max = new Point3d(-INF, -INF, -INF);
    Point3d min = new Point3d(INF, INF, INF);
    switch(method) {
        case ConvexHull:
            {
                HashedPointSet pointSet = createPointSetForOBB(elems, num);
                quickhull3d.Point3d[] hullPnts = computeConvexHullAndCovariance(C, cent, pointSet.getPointsAsDoubleArray(), pointSet.size());
                setTransform(C, cent);
                computeBoundsFromConvexHullPoints(min, max, hullPnts, hullPnts.length);
                break;
            }
        case Covariance:
            {
                computeCovarianceFromElements(C, cent, elems, num);
                setTransform(C, cent);
                computeBoundsFromElements(min, max, elems, num);
                break;
            }
        case Points:
            {
                HashedPointSet pointSet = createPointSetForOBB(elems, num);
                Point3d[] pnts = pointSet.getPoints();
                computeCovarianceFromPoints(C, cent, pnts);
                setTransform(C, cent);
                computeBoundsFromPoints(min, max, pnts);
                break;
            }
        default:
            throw new InternalErrorException("Unimplemented method " + method);
    }
    setWidthsAndCenter(min, max, margin);
}