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

Example 11 with Matrix6d

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

the class MechSystemSolver method rotate.

private void rotate(Matrix6d MR, Matrix6d M1, RotationMatrix3d R) {
    Matrix6d RR = new Matrix6d();
    RR.setSubMatrix00(R);
    RR.setSubMatrix33(R);
    MR.mul(RR, M1);
    MR.mulTransposeRight(MR, RR);
}

Example 12 with Matrix6d

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

the class FrameSpring method addVelJacobianWorld.

public void addVelJacobianWorld(SparseNumberedBlockMatrix M, double s) {
    FrameMaterial mat = myMaterial;
    if (mat == null) {
        // just in case implementation allows null material ...
        return;
    }
    Matrix6dBlock blk00 = getBlock(M, myBlk00Num);
    Matrix6dBlock blk11 = getBlock(M, myBlk11Num);
    Matrix6dBlock blk01 = getBlock(M, myBlk01Num);
    Matrix6dBlock blk10 = getBlock(M, myBlk10Num);
    Matrix6d D = new Matrix6d();
    RotationMatrix3d R1W = new RotationMatrix3d();
    RigidTransform3d XAW = myFrameA.getPose();
    RigidTransform3d XBW;
    if (myFrameB != null) {
        XBW = myFrameB.getPose();
    } else {
        XBW = RigidTransform3d.IDENTITY;
    }
    R1W.mul(XAW.R, myX1A.R);
    Vector3d p1 = new Vector3d();
    Vector3d p2 = new Vector3d();
    p1.transform(XAW.R, myX1A.p);
    p2.transform(XBW.R, myX2B.p);
    // Matrix3d T = myTmpM;
    computeRelativeDisplacements();
    mat.computeDFdu(D, myX21, myVel21, myInitialX21, mySymmetricJacobian);
    D.transform(R1W);
    D.scale(-s);
    if (blk00 != null) {
        myTmp00.set(D);
        postMulMoment(myTmp00, p1);
        preMulMoment(myTmp00, p1);
        blk00.add(myTmp00);
    }
    if (blk11 != null) {
        myTmp11.set(D);
        postMulMoment(myTmp11, p2);
        preMulMoment(myTmp11, p2);
        blk11.add(myTmp11);
    }
    if (blk01 != null && blk10 != null) {
        D.negate();
        myTmp01.set(D);
        postMulMoment(myTmp01, p2);
        preMulMoment(myTmp01, p1);
        myTmp10.set(D);
        postMulMoment(myTmp10, p1);
        preMulMoment(myTmp10, p2);
        blk01.add(myTmp01);
        blk10.add(myTmp10);
    }
}

Example 13 with Matrix6d

use of maspack.matrix.Matrix6d 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");
}

Example 14 with Matrix6d

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

the class FemModel3d method computeStressAndStiffness.

// DIVBLK
private void computeStressAndStiffness(FemElement3d e, FemMaterial mat, Matrix6d D, IncompMethod softIncomp) {
    IntegrationPoint3d[] ipnts = e.getIntegrationPoints();
    IntegrationData3d[] idata = e.getIntegrationData();
    FemNode3d[] nodes = e.getNodes();
    if (D != null) {
        D.setZero();
    }
    SolidDeformation def = new SolidDeformation();
    // ===========================================
    // linear material optimizations
    // ===========================================
    // potentially update cached linear material
    // internally updates
    StiffnessWarper3d warper = e.getStiffnessWarper();
    // if there is cached linear material, then apply
    if (!warper.isCacheEmpty()) {
        // compute warping rotation
        warper.computeWarpingRotation(e);
        // add force and stiffness
        for (int i = 0; i < nodes.length; i++) {
            int bi = nodes[i].getSolveIndex();
            if (bi != -1) {
                FemNode3d n = nodes[i];
                if (!myStiffnessesValidP) {
                    for (int j = 0; j < nodes.length; j++) {
                        int bj = nodes[j].getSolveIndex();
                        if (!mySolveMatrixSymmetricP || bj >= bi) {
                            warper.addNodeStiffness(e.myNbrs[i][j].getK(), i, j);
                        }
                    }
                }
                // add node force
                warper.addNodeForce(n.myInternalForce, i, nodes);
            }
        }
        if (myComputeNodalStress || (myComputeNodalStrain && mat.isLinear())) {
            // estimate at warping point
            RotationMatrix3d R = warper.getRotation();
            IntegrationPoint3d wpnt = e.getWarpingPoint();
            IntegrationData3d wdata = e.getWarpingData();
            wpnt.computeJacobianAndGradient(nodes, wdata.myInvJ0);
            def.clear();
            def.setF(wpnt.F);
            def.setAveragePressure(0);
            def.setR(R);
            SymmetricMatrix3d sigma = new SymmetricMatrix3d();
            SymmetricMatrix3d tmp = new SymmetricMatrix3d();
            // compute nodal stress at wpnt
            if (myComputeNodalStress) {
                // compute linear stress
                mat.computeStress(tmp, def, null, null);
                sigma.add(tmp);
                for (AuxiliaryMaterial amat : e.getAuxiliaryMaterials()) {
                    amat.computeStress(tmp, def, wpnt, e.getWarpingData(), mat);
                    sigma.add(tmp);
                }
                // distribute stress to nodes
                for (int i = 0; i < nodes.length; i++) {
                    nodes[i].addScaledStress(1.0 / nodes[i].numAdjacentElements(), sigma);
                }
            }
            if (myComputeNodalStrain && mat.isLinear()) {
                // Cauchy strain at warping point
                if (mat.isCorotated()) {
                    // remove rotation from F
                    sigma.mulTransposeLeftSymmetric(R, wpnt.F);
                } else {
                    sigma.setSymmetric(wpnt.F);
                }
                sigma.m00 -= 1;
                sigma.m11 -= 1;
                sigma.m22 -= 1;
                // distribute strain to nodes
                for (int i = 0; i < nodes.length; i++) {
                    nodes[i].addScaledStrain(1.0 / nodes[i].numAdjacentElements(), sigma);
                }
            }
        }
    // stress or strain
    }
    // exit early if no non-linear materials
    boolean linearOnly = mat.isLinear();
    if (linearOnly) {
        for (AuxiliaryMaterial amat : e.getAuxiliaryMaterials()) {
            if (!amat.isLinear()) {
                linearOnly = false;
                break;
            }
        }
    }
    if (linearOnly) {
        return;
    }
    // we have some non-linear contributions
    // will check this below
    e.setInverted(false);
    // ===========================================
    // non-linear materials
    // ===========================================
    // temporary stress and tangent
    SymmetricMatrix3d sigmaTmp = new SymmetricMatrix3d();
    Matrix6d Dtmp = null;
    if (D != null) {
        Dtmp = new Matrix6d();
    }
    // viscoelastic behaviour
    ViscoelasticBehavior veb = mat.getViscoBehavior();
    double vebTangentScale = 1;
    if (veb != null) {
        vebTangentScale = veb.getTangentScale();
    }
    // incompressibility
    IncompressibleMaterial imat = null;
    if (mat.isIncompressible()) {
        imat = (IncompressibleMaterial) mat;
    }
    MatrixBlock[] constraints = null;
    SymmetricMatrix3d C = new SymmetricMatrix3d();
    // initialize incompressible pressure
    double[] pbuf = myPressures.getBuffer();
    if (mat.isIncompressible() && softIncomp == IncompMethod.ELEMENT) {
        computePressuresAndRinv(e, imat, vebTangentScale);
        if (D != null) {
            constraints = e.getIncompressConstraints();
            for (int i = 0; i < e.myNodes.length; i++) {
                constraints[i].setZero();
            }
        }
    }
    // cache invertible flag
    boolean invertibleMaterials = e.materialsAreInvertible();
    // loop through each integration point
    for (int k = 0; k < ipnts.length; k++) {
        IntegrationPoint3d pt = ipnts[k];
        IntegrationData3d dt = idata[k];
        double scaling = dt.getScaling();
        pt.computeJacobianAndGradient(e.myNodes, idata[k].myInvJ0);
        def.clear();
        def.setF(pt.F);
        def.setAveragePressure(0);
        def.setR(null);
        double detJ = pt.computeInverseJacobian();
        if (detJ < myMinDetJ) {
            myMinDetJ = detJ;
            myMinDetJElement = e;
        }
        if (detJ <= 0 && !invertibleMaterials) {
            e.setInverted(true);
            myNumInverted++;
        }
        // compute shape function gradient and volume fraction
        double dv = detJ * pt.getWeight();
        Vector3d[] GNx = pt.updateShapeGradient(pt.myInvJ);
        // compute pressure
        double pressure = 0;
        double[] H = null;
        if (mat.isIncompressible()) {
            if (softIncomp == IncompMethod.ELEMENT) {
                H = pt.getPressureWeights().getBuffer();
                int npvals = e.numPressureVals();
                for (int l = 0; l < npvals; l++) {
                    pressure += H[l] * pbuf[l];
                }
            } else if (softIncomp == IncompMethod.NODAL) {
                if (e instanceof TetElement) {
                    // use the average pressure for all nodes
                    pressure = 0;
                    for (int i = 0; i < nodes.length; i++) {
                        pressure += nodes[i].myPressure;
                    }
                    pressure /= nodes.length;
                } else if (e.integrationPointsMapToNodes()) {
                    pressure = nodes[k].myPressure;
                } else if (e.integrationPointsInterpolateToNodes()) {
                    // interpolate using shape function
                    VectorNd N = pt.getShapeWeights();
                    pressure = 0;
                    for (int i = 0; i < N.size(); ++i) {
                        pressure += nodes[i].myPressure * N.get(i);
                    }
                }
            } else if (softIncomp == IncompMethod.FULL) {
                pressure = imat.getEffectivePressure(detJ / dt.getDetJ0());
            }
        }
        // anisotropy rotational frame
        Matrix3d Q = (dt.myFrame != null ? dt.myFrame : Matrix3d.IDENTITY);
        // System.out.println("FEM Pressure: " + pressure);
        pt.avgp = pressure;
        def.setAveragePressure(pressure);
        // clear stress/tangents
        pt.sigma.setZero();
        if (D != null) {
            D.setZero();
        }
        // base material
        if (!mat.isLinear()) {
            mat.computeStress(pt.sigma, def, Q, null);
            if (scaling != 1) {
                pt.sigma.scale(scaling);
            }
            if (D != null) {
                mat.computeTangent(D, pt.sigma, def, Q, null);
                if (scaling != 1) {
                    D.scale(scaling);
                }
            }
        }
        // reset pressure to zero for auxiliary materials
        pt.avgp = 0;
        def.setAveragePressure(0);
        if (e.numAuxiliaryMaterials() > 0) {
            for (AuxiliaryMaterial amat : e.getAuxiliaryMaterials()) {
                // skip linear materials
                if (!amat.isLinear()) {
                    amat.computeStress(sigmaTmp, def, pt, dt, mat);
                    if (scaling != 1) {
                        sigmaTmp.scale(scaling);
                    }
                    pt.sigma.add(sigmaTmp);
                    if (D != null) {
                        amat.computeTangent(Dtmp, sigmaTmp, def, pt, dt, mat);
                        if (scaling != 1) {
                            Dtmp.scale(scaling);
                        }
                        D.add(Dtmp);
                    }
                }
            }
        }
        // XXX only uses non-linear stress
        // bring back pressure term
        pt.avgp = pressure;
        def.setAveragePressure(pressure);
        if (veb != null) {
            ViscoelasticState state = idata[k].getViscoState();
            if (state == null) {
                state = veb.createState();
                idata[k].setViscoState(state);
            }
            veb.computeStress(pt.sigma, state);
            if (D != null) {
                veb.computeTangent(D, state);
            }
        } else {
            dt.clearState();
        }
        // sum stress/stiffness contributions to each node
        for (int i = 0; i < e.myNodes.length; i++) {
            FemNode3d nodei = e.myNodes[i];
            int bi = nodei.getSolveIndex();
            FemUtilities.addStressForce(nodei.myInternalForce, GNx[i], pt.sigma, dv);
            if (D != null) {
                double p = 0;
                double kp = 0;
                if (mat.isIncompressible()) {
                    if (softIncomp == IncompMethod.ELEMENT) {
                        FemUtilities.addToIncompressConstraints(constraints[i], H, GNx[i], dv);
                        p = pressure;
                    } else if (softIncomp == IncompMethod.FULL) {
                        double dV = dt.getDetJ0() * pt.getWeight();
                        kp = imat.getEffectiveModulus(detJ / dt.getDetJ0()) * dV;
                        p = pressure;
                    }
                }
                // compute stiffness
                if (bi != -1) {
                    for (int j = 0; j < e.myNodes.length; j++) {
                        int bj = e.myNodes[j].getSolveIndex();
                        if (!mySolveMatrixSymmetricP || bj >= bi) {
                            e.myNbrs[i][j].addMaterialStiffness(GNx[i], D, GNx[j], dv);
                            e.myNbrs[i][j].addGeometricStiffness(GNx[i], pt.sigma, GNx[j], dv);
                            e.myNbrs[i][j].addPressureStiffness(GNx[i], p, GNx[j], dv);
                            if (kp != 0) {
                                e.myNbrs[i][j].addDilationalStiffness(vebTangentScale * kp, GNx[i], GNx[j]);
                            }
                        }
                    }
                }
            }
            // if D != null
            // nodal stress/strain
            double[] nodalExtrapMat = e.getNodalExtrapolationMatrix();
            if (nodalExtrapMat != null) {
                if (myComputeNodalStress) {
                    double a = nodalExtrapMat[i * ipnts.length + k];
                    if (a != 0) {
                        nodei.addScaledStress(a / nodei.numAdjacentElements(), pt.sigma);
                    }
                }
                // if base material non-linear and computing nodal strain
                if (myComputeNodalStrain && !mat.isLinear()) {
                    double a = nodalExtrapMat[i * ipnts.length + k];
                    if (a != 0) {
                        def.computeRightCauchyGreen(C);
                        C.m00 -= 1;
                        C.m11 -= 1;
                        C.m22 -= 1;
                        C.scale(0.5);
                        nodei.addScaledStrain(a / nodei.numAdjacentElements(), C);
                    }
                }
            }
        }
        // nodal incompressibility constraints
        if (D != null && softIncomp == IncompMethod.NODAL && !(e instanceof TetElement)) {
            if (e.integrationPointsMapToNodes()) {
                for (FemNodeNeighbor nbr : getNodeNeighbors(e.myNodes[k])) {
                    int j = e.getLocalNodeIndex(nbr.myNode);
                    if (j != -1) {
                        // myNodalConstraints[i].scale(dv, GNx[i]);
                        nbr.myDivBlk.scaledAdd(dv, GNx[j]);
                    }
                }
            } else if (e.integrationPointsInterpolateToNodes()) {
                // distribute according to shape function weights
                VectorNd N = pt.getShapeWeights();
                for (int i = 0; i < N.size(); ++i) {
                    for (FemNodeNeighbor nbr : getNodeNeighbors(e.myNodes[i])) {
                        int j = e.getLocalNodeIndex(nbr.getNode());
                        if (j != -1) {
                            nbr.myDivBlk.scaledAdd(N.get(i) * dv, GNx[j]);
                        }
                    }
                }
            }
        }
    // soft incompressibility
    }
    // tet nodal incompressibility
    if (D != null && mat.isIncompressible() && softIncomp == IncompMethod.NODAL) {
        if (e instanceof TetElement) {
            ((TetElement) e).getAreaWeightedNormals(myNodalConstraints);
            for (int i = 0; i < 4; i++) {
                myNodalConstraints[i].scale(-1 / 12.0);
            }
            for (int i = 0; i < e.numNodes(); ++i) {
                for (FemNodeNeighbor nbr : getNodeNeighbors(e.myNodes[i])) {
                    int j = e.getLocalNodeIndex(nbr.myNode);
                    if (j != -1) {
                        nbr.myDivBlk.scaledAdd(1, myNodalConstraints[j]);
                    }
                }
            }
        }
    }
    // element-wise incompressibility constraints
    if (D != null) {
        if (mat.isIncompressible() && softIncomp == IncompMethod.ELEMENT) {
            boolean kpIsNonzero = false;
            int npvals = e.numPressureVals();
            for (int l = 0; l < npvals; l++) {
                double Jpartial = e.myVolumes[l] / e.myRestVolumes[l];
                myKp[l] = imat.getEffectiveModulus(Jpartial) / e.myRestVolumes[l];
                if (myKp[l] != 0) {
                    kpIsNonzero = true;
                }
            }
            // double kp = imat.getEffectiveModulus(vol/restVol)/restVol;
            if (true) {
                for (int i = 0; i < e.myNodes.length; i++) {
                    int bi = e.myNodes[i].getSolveIndex();
                    if (bi != -1) {
                        for (int j = 0; j < e.myNodes.length; j++) {
                            int bj = e.myNodes[j].getSolveIndex();
                            if (!mySolveMatrixSymmetricP || bj >= bi) {
                                e.myNbrs[i][j].addDilationalStiffness(myRinv, constraints[i], constraints[j]);
                            }
                        // end filling in symmetric
                        }
                    // end filling in dilatational stiffness
                    }
                // end checking if valid index
                }
            // end looping through nodes
            }
        // XXX ALWAYS??
        }
    // end soft elem incompress
    }
// end checking if computing tangent
}

Example 15 with Matrix6d

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

the class StiffnessWarper3d method computeInitialStiffness.

@Deprecated
public void computeInitialStiffness(FemElement3d e, double E, double nu, boolean corotated) {
    IntegrationPoint3d[] ipnts = e.getIntegrationPoints();
    IntegrationData3d[] idata = e.getIntegrationData();
    Matrix6d D = new Matrix6d();
    double dia = (1 - nu) / (1 - 2 * nu);
    double off = nu / (1 - 2 * nu);
    D.m00 = dia;
    D.m01 = off;
    D.m02 = off;
    D.m10 = off;
    D.m11 = dia;
    D.m12 = off;
    D.m20 = off;
    D.m21 = off;
    D.m22 = dia;
    D.m33 = 0.5;
    D.m44 = 0.5;
    D.m55 = 0.5;
    D.scale(E / (1 + nu));
    Vector3d tmp = new Vector3d();
    LinearMaterialCache cache;
    if (corotated) {
        cache = getOrCreateCorotatedCache();
    } else {
        cache = getOrCreateLinearCache();
    }
    for (int i = 0; i < e.myNodes.length; i++) {
        for (int j = 0; j < e.myNodes.length; j++) {
            cache.K0[i][j].setZero();
        }
        cache.f0[i].setZero();
    }
    for (int k = 0; k < ipnts.length; k++) {
        IntegrationPoint3d pt = ipnts[k];
        IntegrationData3d dt = idata[k];
        double dv = dt.myDetJ0 * pt.getWeight();
        if (dt.myScaling != 1) {
            dv *= dt.myScaling;
        }
        Vector3d[] GNx = pt.updateShapeGradient(dt.myInvJ0);
        for (int i = 0; i < e.myNodes.length; i++) {
            for (int j = 0; j < e.myNodes.length; j++) {
                FemUtilities.addMaterialStiffness(cache.K0[i][j], GNx[i], D, SymmetricMatrix3d.ZERO, GNx[j], dv);
            }
        }
    }
    for (int i = 0; i < e.myNodes.length; i++) {
        tmp.setZero();
        for (int j = 0; j < e.myNodes.length; j++) {
            cache.K0[i][j].mulAdd(tmp, e.myNodes[j].myRest, tmp);
        }
        cache.f0[i].set(tmp);
    }
}