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

Example 21 with Matrix3d

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

the class FrameSpring method addPosJacobianWorld.

public void addPosJacobianWorld(SparseNumberedBlockMatrix M, double s) {
    FrameMaterial mat = myMaterial;
    if (mat == null) {
        // just in case implementation allows null material ...
        return;
    }
    Matrix6d JK = new Matrix6d();
    Matrix6d JD = null;
    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();
    Vector3d x21 = new Vector3d();
    p1.transform(XAW.R, myX1A.p);
    p2.transform(XBW.R, myX2B.p);
    Matrix6dBlock blk00 = getBlock(M, myBlk00Num);
    Matrix6dBlock blk11 = getBlock(M, myBlk11Num);
    Matrix6dBlock blk01 = getBlock(M, myBlk01Num);
    Matrix6dBlock blk10 = getBlock(M, myBlk10Num);
    Matrix3d T = myTmpM;
    computeRelativeDisplacements();
    x21.set(myX21.p);
    x21.transform(R1W);
    myFrameA.getVelocity(myVel1);
    if (myFrameB != null) {
        myFrameB.getVelocity(myVel2);
    } else {
        myVel2.setZero();
    }
    if (!mySymmetricJacobian) {
        mat.computeF(myF, myX21, myVel21, myInitialX21);
        // map forces back to World coords
        myF.transform(R1W);
        JD = new Matrix6d();
        mat.computeDFdu(JD, myX21, myVel21, myInitialX21, mySymmetricJacobian);
        JD.transform(R1W);
    }
    mat.computeDFdq(JK, myX21, myVel21, myInitialX21, mySymmetricJacobian);
    JK.transform(R1W);
    JK.scale(-s);
    myVel21.transform(R1W);
    if (blk00 != null) {
        myTmp00.set(JK);
        postMulMoment(myTmp00, p1);
    }
    if (blk11 != null) {
        myTmp11.set(JK);
        postMulMoment(myTmp11, p2);
    }
    if (blk01 != null && blk10 != null) {
        JK.negate();
        myTmp01.set(JK);
        postMulMoment(myTmp01, p2);
        myTmp10.set(JK);
        postMulMoment(myTmp10, p1);
    }
    if (blk00 != null) {
        if (!mySymmetricJacobian) {
            // NEW
            postMulMoment(myTmp00, x21);
            setScaledCrossProd(T, -s, myF.f);
            myTmp00.addSubMatrix03(T);
            setScaledCrossProd(T, -s, myF.m);
            myTmp00.addSubMatrix33(T);
            postMulWrenchCross(myTmp00, JD, s, myVel21);
            setScaledCrossProd(T, s, p1);
            T.crossProduct(myF.f, T);
            myTmp00.addSubMatrix33(T);
            // setScaledCrossProd (T, -s, myFk.m);
            // myTmp00.addSubMatrix33 (T);
            setScaledCrossProd(T, s, p1);
            T.crossProduct(myVel1.w, T);
            postMul03Block(myTmp00, JD, T);
            if (blk10 != null) {
                // NEW
                postMulMoment(myTmp10, x21);
                setScaledCrossProd(T, s, myF.f);
                myTmp10.addSubMatrix03(T);
                setScaledCrossProd(T, s, myF.m);
                myTmp10.addSubMatrix33(T);
                postMulWrenchCross(myTmp10, JD, -s, myVel21);
                setScaledCrossProd(T, -s, p1);
                T.crossProduct(myVel1.w, T);
                postMul03Block(myTmp10, JD, T);
            // setScaledCrossProd (T, s, myFk.m);
            // myTmp10.addSubMatrix33 (T);
            }
        }
        preMulMoment(myTmp00, p1);
        blk00.add(myTmp00);
    }
    if (blk11 != null) {
        if (!mySymmetricJacobian) {
            setScaledCrossProd(T, -s, p2);
            T.crossProduct(myF.f, T);
            myTmp11.addSubMatrix33(T);
            setScaledCrossProd(T, s, p2);
            T.crossProduct(myVel2.w, T);
            postMul03Block(myTmp11, JD, T);
            if (blk01 != null) {
                T.negate();
                postMul03Block(myTmp01, JD, T);
            }
        }
        preMulMoment(myTmp11, p2);
        blk11.add(myTmp11);
    }
    if (blk01 != null && blk10 != null) {
        preMulMoment(myTmp01, p1);
        preMulMoment(myTmp10, p2);
        blk01.add(myTmp01);
        blk10.add(myTmp10);
    }
}

Example 22 with Matrix3d

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

the class FrameSpring method copy.

@Override
public ModelComponent copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    FrameSpring comp = (FrameSpring) super.copy(flags, copyMap);
    if (myRenderProps != null) {
        comp.setRenderProps(myRenderProps);
    }
    if (myX1A != RigidTransform3d.IDENTITY) {
        comp.myX1A = new RigidTransform3d(myX1A);
    }
    if (myX2B != RigidTransform3d.IDENTITY) {
        comp.myX2B = new RigidTransform3d(myX2B);
    }
    comp.myF = new Wrench();
    comp.myFTmp = new Wrench();
    comp.myVel1 = new Twist();
    comp.myVel2 = new Twist();
    comp.myVel21 = new Twist();
    comp.myTmp00 = new Matrix6d();
    comp.myTmp01 = new Matrix6d();
    comp.myTmp10 = new Matrix6d();
    comp.myTmp11 = new Matrix6d();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myRenderPnt1 = new float[3];
    comp.myRenderPnt2 = new float[3];
    comp.myTmpM = new Matrix3d();
    // comp.myRBA = new RotationMatrix3d();
    comp.myX21 = new RigidTransform3d();
    comp.myMaterial = (FrameMaterial) myMaterial.clone();
    return comp;
}

Example 23 with Matrix3d

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

the class StVenantKirchoffMaterial method main.

public static void main(String[] args) {
    StVenantKirchoffMaterial mat = new StVenantKirchoffMaterial();
    SolidDeformation def = new SolidDeformation();
    Matrix3d Q = new Matrix3d();
    def.setF(new Matrix3d(1, 3, 5, 2, 1, 4, 6, 1, 2));
    Matrix6d D = new Matrix6d();
    SymmetricMatrix3d sig = new SymmetricMatrix3d();
    mat.setYoungsModulus(10);
    mat.computeStress(sig, def, Q, null);
    mat.computeTangent(D, sig, def, Q, null);
    System.out.println("sig=\n" + sig.toString("%12.6f"));
    System.out.println("D=\n" + D.toString("%12.6f"));
}

Example 24 with Matrix3d

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

the class MFreeElement3d method addIntegrationPoint.

public void addIntegrationPoint(MFreeIntegrationPoint3d ipnt, IntegrationData3d idata, double iwgt, boolean updateVolumes) {
    if (idata == null) {
        idata = new IntegrationData3d();
        idata.setRestInverseJacobian(new Matrix3d(Matrix3d.IDENTITY), 1);
    }
    int[] idxs = new int[numNodes()];
    for (int j = 0; j < numNodes(); j++) {
        idxs[j] = ipnt.getNodeCoordIdx(myNodes[j]);
        if (idxs[j] < 0) {
            // XXX happens when right on the boundary
            return;
        }
    }
    // int idx = myIntegrationWeights.adjustSize(1);
    // myIntegrationWeights.set(idx, iwgt);
    myIntegrationData.add(idata);
    // ipnt.setNumber(myIntegrationPoints.size());
    ipnt.setNumber(myIntegrationPoints.size());
    myIntegrationPoints.add(ipnt);
    // myIntegrationNodeIdxs.add(idxs);
    myNodalExtrapolationMatrix = null;
    if (updateVolumes) {
        updateAllVolumes();
    }
}

Example 25 with Matrix3d

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

the class MFreeElement3d method getNaturalCoordinates.

/**
 * Given point p, get its natural coordinates with respect to this element.
 * Returns true if the algorithm converges, false if a maximum number of
 * iterations is reached. Uses a modified Newton's method to solve the
 * equations. The <code>coords</code> argument that returned the coordinates is
 * used, on input, to supply an initial guess of the coordinates.
 * Zero is generally a safe guess.
 *
 * @param coords
 * Outputs the natural coordinates, and supplies (on input) an initial
 * guess as to their position.
 * @param pnt
 * A given point (in world coords)
 * @param maxIters
 * Maximum number of Newton iterations
 * @param N
 * Resulting shape functionvalues
 * @return the number of iterations required for convergence, or
 * -1 if the calculation did not converge.
 */
public int getNaturalCoordinates(Point3d coords, Point3d pnt, int maxIters, VectorNd N) {
    Vector3d res = new Point3d();
    int i;
    double tol = ((MFreeNode3d) myNodes[0]).getInfluenceRadius() * 1e-12;
    if (tol <= 0) {
        tol = 1e-12;
    }
    if (N == null) {
        N = new VectorNd(numNodes());
    } else {
        N.setSize(numNodes());
    }
    // if (!coordsAreInside(coords)) {
    // return -1;
    // }
    myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
    computeNaturalCoordsResidual(res, coords, pnt, N);
    double prn = res.norm();
    // System.out.println ("res=" + prn);
    if (prn < tol) {
        // already have the right answer
        return 0;
    }
    LUDecomposition LUD = new LUDecomposition();
    Vector3d prevCoords = new Vector3d();
    Vector3d dNds = new Vector3d();
    Matrix3d dxds = new Matrix3d();
    Vector3d del = new Point3d();
    /*
       * solve using Newton's method.
       */
    for (i = 0; i < maxIters; i++) {
        // compute the Jacobian dx/ds for the current guess
        dxds.setZero();
        for (int k = 0; k < numNodes(); k++) {
            myShapeFunction.evalDerivative(k, dNds);
            dxds.addOuterProduct(myNodes[k].getPosition(), dNds);
        }
        LUD.factor(dxds);
        double cond = LUD.conditionEstimate(dxds);
        if (cond > 1e10)
            System.err.println("Warning: condition number for solving natural coordinates is " + cond);
        // solve Jacobian to obtain an update for the coords
        LUD.solve(del, res);
        prevCoords.set(coords);
        coords.sub(del);
        // if (!coordsAreInside(coords)) {
        // return -1;
        // }
        myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
        computeNaturalCoordsResidual(res, coords, pnt, N);
        double rn = res.norm();
        // If the residual norm is within tolerance, we have converged.
        if (rn < tol) {
            // System.out.println ("2 res=" + rn);
            return i + 1;
        }
        if (rn > prn) {
            // it may be that "coords + del" is a worse solution.  Let's make
            // sure we go the correct way binary search suitable alpha in [0 1]
            double eps = 1e-12;
            // and keep cutting the step size in half until the residual starts
            // dropping again
            double alpha = 0.5;
            while (alpha > eps && rn > prn) {
                coords.scaledAdd(-alpha, del, prevCoords);
                if (!coordsAreInside(coords)) {
                    return -1;
                }
                myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
                computeNaturalCoordsResidual(res, coords, pnt, N);
                rn = res.norm();
                alpha *= 0.5;
            // System.out.println ("  alpha=" + alpha + " rn=" + rn);
            }
            // System.out.println (" alpha=" + alpha + " rn=" + rn + " prn=" + prn);
            if (alpha < eps) {
                // failed
                return -1;
            }
        }
        prn = rn;
    }
    // failed
    return -1;
}