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

Example 1 with NumericalException

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

the class PolygonalMesh method computeVolumeIntegrals.

/**
 * Computes the volume integrals of this mesh, on the assumption that it is
 * manifold and closed. The code for this was taken from vclip, by Brian
 * Mirtich. See "Fast and Accurate Computation of Polyhedral Mass
 * Properties," Brian Mirtich, journal of graphics tools, volume 1, number 2,
 * 1996.
 *
 * @param mov1
 * if non-null, returns the first moment of volume
 * @param mov2
 * if non-null, returns the second moment of volume
 * @param pov
 * if non-null, returns the product of volume
 * @return closed volume of the mesh
 */
public double computeVolumeIntegrals(Vector3d mov1, Vector3d mov2, Vector3d pov) {
    int a, b, c;
    // Edge e;
    // Face f;
    double a0, a1, da;
    // al
    double b0, b1, db;
    double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
    double a1_2, a1_3, b1_2, b1_3;
    double d, na, nb, nc, inv;
    double I, Ia, Ib, Iaa, Iab, Ibb, Iaaa, Iaab, Iabb, Ibbb;
    double Icc, Iccc, Ibbc, Icca;
    double C0, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
    double Cab, Kab, Caab, Kaab, Cabb, Kabb;
    // h, w;
    Vector3d v;
    if (mov1 != null) {
        mov1.setZero();
    }
    if (mov2 != null) {
        mov2.setZero();
    }
    if (pov != null) {
        pov.setZero();
    }
    // double rad_ = 0;
    double vol_ = 0.0;
    for (int i = 0; i < myFaces.size(); i++) {
        Face f = myFaces.get(i);
        // compute projection direction
        Vector3d nrml = f.getNormal();
        if (nrml.containsNaN()) {
            // sanity check for badly formed meshes
            continue;
        }
        v = new Vector3d();
        v.set(Math.abs(nrml.x), Math.abs(nrml.y), Math.abs(nrml.z));
        c = (v.x >= v.y) ? ((v.x >= v.z) ? 0 : 2) : ((v.y >= v.z) ? 1 : 2);
        a = (c + 1) % 3;
        b = (c + 2) % 3;
        I = Ia = Ib = Iaa = Iab = Ibb = Iaaa = Iaab = Iabb = Ibbb = 0.0;
        // walk around face
        HalfEdge he0 = f.firstHalfEdge();
        HalfEdge he = he0;
        do {
            a0 = he.getTail().pnt.get(a);
            b0 = he.getTail().pnt.get(b);
            a1 = he.getHead().pnt.get(a);
            b1 = he.getHead().pnt.get(b);
            da = a1 - a0;
            db = b1 - b0;
            a0_2 = a0 * a0;
            a0_3 = a0_2 * a0;
            a0_4 = a0_3 * a0;
            b0_2 = b0 * b0;
            b0_3 = b0_2 * b0;
            b0_4 = b0_3 * b0;
            a1_2 = a1 * a1;
            a1_3 = a1_2 * a1;
            b1_2 = b1 * b1;
            b1_3 = b1_2 * b1;
            C0 = a1 + a0;
            Ca = a1 * C0 + a0_2;
            Caa = a1 * Ca + a0_3;
            Caaa = a1 * Caa + a0_4;
            Cb = b1 * (b1 + b0) + b0_2;
            Cbb = b1 * Cb + b0_3;
            Cbbb = b1 * Cbb + b0_4;
            Cab = 3 * a1_2 + 2 * a1 * a0 + a0_2;
            Kab = a1_2 + 2 * a1 * a0 + 3 * a0_2;
            Caab = a0 * Cab + 4 * a1_3;
            Kaab = a1 * Kab + 4 * a0_3;
            Cabb = 4 * b1_3 + 3 * b1_2 * b0 + 2 * b1 * b0_2 + b0_3;
            Kabb = b1_3 + 2 * b1_2 * b0 + 3 * b1 * b0_2 + 4 * b0_3;
            I += db * C0;
            Ia += db * Ca;
            Iaa += db * Caa;
            Iaaa += db * Caaa;
            Ib += da * Cb;
            Ibb += da * Cbb;
            Ibbb += da * Cbbb;
            Iab += db * (b1 * Cab + b0 * Kab);
            Iaab += db * (b1 * Caab + b0 * Kaab);
            Iabb += da * (a1 * Cabb + a0 * Kabb);
            he = he.getNext();
        } while (he != he0);
        I /= 2.0;
        Ia /= 6.0;
        Iaa /= 12.0;
        Iaaa /= 20.0;
        Ib /= -6.0;
        Ibb /= -12.0;
        Ibbb /= -20.0;
        Iab /= 24.0;
        Iaab /= 60.0;
        Iabb /= -60.0;
        d = -nrml.dot(f.firstHalfEdge().getHead().pnt);
        na = nrml.get(a);
        nb = nrml.get(b);
        nc = nrml.get(c);
        inv = 1.0 / nc;
        if (a == 0) {
            vol_ += inv * na * Ia;
        } else if (b == 0) {
            vol_ += inv * nb * Ib;
        } else {
            vol_ -= ((d * I + na * Ia + nb * Ib) / nc);
        }
        if (vol_ != vol_) {
            throw new NumericalException("nrml=" + nrml + ", face " + i);
        }
        Icc = (SQR(na) * Iaa + 2 * na * nb * Iab + SQR(nb) * Ibb + d * (2 * (na * Ia + nb * Ib) + d * I)) * SQR(inv);
        if (mov1 != null) {
            mov1.set(a, mov1.get(a) + inv * na * Iaa);
            mov1.set(b, mov1.get(b) + inv * nb * Ibb);
            mov1.set(c, mov1.get(c) + Icc);
        }
        Iccc = -(CUBE(na) * Iaaa + 3 * SQR(na) * nb * Iaab + 3 * na * SQR(nb) * Iabb + CUBE(nb) * Ibbb + 3 * (SQR(na) * Iaa + 2 * na * nb * Iab + SQR(nb) * Ibb) * d + d * d * (3 * (na * Ia + nb * Ib) + d * I)) * CUBE(inv);
        if (mov2 != null) {
            mov2.set(a, mov2.get(a) + inv * na * Iaaa);
            mov2.set(b, mov2.get(b) + inv * nb * Ibbb);
            mov2.set(c, mov2.get(c) + Iccc);
        }
        Ibbc = -(d * Ibb + na * Iabb + nb * Ibbb) * inv;
        Icca = (SQR(na) * Iaaa + 2 * na * nb * Iaab + SQR(nb) * Iabb + d * (2 * (na * Iaa + nb * Iab) + d * Ia)) * SQR(inv);
        if (pov != null) {
            pov.set(c, pov.get(c) + inv * na * Iaab);
            pov.set(a, pov.get(a) + inv * nb * Ibbc);
            pov.set(b, pov.get(b) + Icca);
        }
    }
    if (mov1 != null) {
        mov1.scale(0.5);
    }
    if (mov2 != null) {
        mov2.scale(1.0 / 3.0);
    }
    if (pov != null) {
        pov.scale(0.5);
    }
    return vol_;
}

Example 2 with NumericalException

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

the class FemModel3d method updateStressAndStiffness.

// DIVBLK
public void updateStressAndStiffness() {
    // allocate or deallocate nodal incompressibility blocks
    setNodalIncompBlocksAllocated(getSoftIncompMethod() == IncompMethod.NODAL);
    // clear existing internal forces and maybe stiffnesses
    for (FemNode3d n : myNodes) {
        n.myInternalForce.setZero();
        if (!myStiffnessesValidP) {
            for (FemNodeNeighbor nbr : getNodeNeighbors(n)) {
                nbr.zeroStiffness();
            }
            // used for soft nodal-based incompressibilty:
            for (FemNodeNeighbor nbr : getIndirectNeighbors(n)) {
                nbr.zeroStiffness();
            }
        }
        if (myComputeNodalStress) {
            n.zeroStress();
        }
        if (myComputeNodalStrain) {
            n.zeroStrain();
        }
    }
    if (!myVolumeValid) {
        updateVolume();
    }
    IncompMethod softIncomp = getSoftIncompMethod();
    if (softIncomp == IncompMethod.NODAL) {
        if (!myNodalRestVolumesValidP) {
            updateNodalRestVolumes();
        }
        setNodalIncompConstraintsAllocated(true);
        updateNodalPressures((IncompressibleMaterial) myMaterial);
        for (FemNode3d n : myNodes) {
            for (FemNodeNeighbor nbr : getNodeNeighbors(n)) {
                nbr.myDivBlk.setZero();
            }
        }
    }
    Matrix6d D = new Matrix6d();
    // compute new forces as well as stiffness matrix if warping is enabled
    myMinDetJ = Double.MAX_VALUE;
    myMinDetJElement = null;
    myNumInverted = 0;
    double mins = Double.MAX_VALUE;
    FemElement3d minE = null;
    for (FemElement3d e : myElements) {
        FemMaterial mat = getElementMaterial(e);
        computeStressAndStiffness(e, mat, D, softIncomp);
        if (checkTangentStability) {
            double s = checkMatrixStability(D);
            if (s < mins) {
                mins = s;
                minE = e;
            }
        }
    }
    // incompressibility
    if ((softIncomp == IncompMethod.NODAL) && myMaterial != null && myMaterial.isIncompressible()) {
        computeNodalIncompressibility((IncompressibleMaterial) myMaterial, D);
    }
    if (checkTangentStability && minE != null) {
        System.out.println("min s=" + mins + ", element " + minE.getNumber());
    }
    if (myNumInverted > 0) {
        if (myWarnOnInvertedElems) {
            System.out.println("Warning: " + myNumInverted + " inverted elements; min detJ=" + myMinDetJ + ", element " + ComponentUtils.getPathName(myMinDetJElement));
        }
        if (myAbortOnInvertedElems) {
            throw new NumericalException("Inverted elements");
        }
    }
    if (!myStiffnessesValidP && mySolveMatrixSymmetricP) {
        for (FemNode3d n : myNodes) {
            int bi = n.getSolveIndex();
            if (bi != -1) {
                for (FemNodeNeighbor nbr : getNodeNeighbors(n)) {
                    int bj = nbr.myNode.getSolveIndex();
                    if (bj > bi) {
                        FemNodeNeighbor nbrT = nbr.myNode.getNodeNeighborBySolveIndex(bi);
                        nbrT.setTransposedStiffness(nbr);
                    }
                }
                // used for soft nodal-based incompressibilty:
                for (FemNodeNeighbor nbr : getIndirectNeighbors(n)) {
                    int bj = nbr.myNode.getSolveIndex();
                    if (bj > bi) {
                        FemNodeNeighbor nbrT = nbr.myNode.getIndirectNeighborBySolveIndex(bi);
                        nbrT.setTransposedStiffness(nbr);
                    }
                }
            }
        }
    }
    myStiffnessesValidP = true;
    myStressesValidP = true;
// timerStop("stressAndStiffness");
}