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

Example 16 with MatrixNd

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

the class GMLSShapeFunction method computeDDM.

@Override
public void computeDDM(MatrixNd DM, int di, int dj, Point3d pnt, MFreeNode3d[] nodeList) {
    MatrixNd cc = new MatrixNd(nBasis, nBasis);
    DM.setSize(nBasis, nBasis);
    DM.setZero();
    int[] derivatives = new int[3];
    derivatives[di] = 1;
    derivatives[dj] = derivatives[dj] + 1;
    for (MFreeNode3d node : nodeList) {
        Point3d pos = node.getRestPosition();
        double w = node.getWeightFunction().evalDerivative(pnt, derivatives);
        for (int i = 0; i <= myOrder; i++) {
            for (int j = 0; j <= myOrder - i; j++) {
                for (int k = 0; k <= myOrder - i - j; k++) {
                    // account for symmetries
                    double d = dcoeff(i, j, k);
                    computeDPCorrelation(cc, pos.x, pos.y, pos.z, i, j, k);
                    DM.scaledAdd(d * w, cc);
                }
            }
        }
    }
}

Example 17 with MatrixNd

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

the class MLSShapeFunction method evalDerivative.

public double evalDerivative(MFreeNode3d node, MFreeNode3d[] nodes, Point3d in, int[] derivatives) {
    MatrixNd MInv = new MatrixNd(nBasis, nBasis);
    computeMInv(MInv, in, nodes);
    return evalDerivative(node, nodes, in, derivatives, MInv);
}

Example 18 with MatrixNd

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

the class MLSShapeFunction method update.

@Override
public void update(Point3d pnt, MFreeNode3d[] nodes) {
    this.myPnt.set(pnt);
    this.myNodes = nodes;
    if (M == null) {
        M = new MatrixNd(nBasis, nBasis);
    }
    if (Minv == null) {
        Minv = new MatrixNd(nBasis, nBasis);
    }
    computeM(M, pnt, nodes);
    SVDecomposition svd = new SVDecomposition(M);
    svd.pseudoInverse(Minv);
    if (svd.condition() > 1e10) {
        System.out.println("Warning: poor condition number, " + svd.condition());
    }
}

Example 19 with MatrixNd

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

the class MLSShapeFunction method computeDDMInv.

// public double eval(double x, double y, double z) {
// Point3d _pnt = new Point3d();
// _pnt.x = x;
// _pnt.y = y;
// _pnt.z = z;
// return eval(_pnt);
// }
// 
// public double eval(double[] in) {
// return eval(in[0], in[1], in[2]);
// }
// 
// public int getInputSize() {
// return 3;
// }
public void computeDDMInv(MatrixNd DDMInv, int di, int dj, MatrixNd MInv, Point3d pnt, MFreeNode3d[] nodeList) {
    MatrixNd mTmpi = new MatrixNd(nBasis, nBasis);
    MatrixNd mTmpj = new MatrixNd(nBasis, nBasis);
    // M^{-1}_{,kl} = M^{-1} [  (M_{,k}M^{-1})(M_{,l}M^{-1}) + (M_{,l}M^{-1})(M_{,k}M^{-1}) - M_{,kl}M^{-1}  ]
    computeDM(mTmpi, di, pnt, nodeList);
    mTmpi.mul(MInv);
    if (di != dj) {
        computeDM(mTmpj, di, pnt, nodeList);
        mTmpj.mul(MInv);
        DDMInv.mul(mTmpi, mTmpj);
        mTmpj.mul(mTmpi);
        DDMInv.add(mTmpj);
    } else {
        DDMInv.mul(mTmpi, mTmpi);
        DDMInv.scale(2);
    }
    computeDDM(mTmpi, di, dj, pnt, nodeList);
    mTmpi.mul(MInv);
    DDMInv.sub(mTmpi);
    DDMInv.mul(MInv, DDMInv);
}

Example 20 with MatrixNd

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

the class SpatialInertia method addPointMass.

/**
 * Adds a point mass at a specified point to a 6x6 matrix representing a
 * spatial inertia. Note that subtracting a point mass can be achieved by
 * specifying a negative mass.
 *
 * @param M
 * Matrix containing the inertia. Must be a Matrix6d or a MatrixNd
 * with size at least 6x6.
 * @param m
 * Mass of the point being added
 * @param pos
 * Point position relative to this inertia's coordinate frame
 */
public static void addPointMass(Matrix M, double m, Vector3d pos) {
    double cx = pos.x;
    double cy = pos.y;
    double cz = pos.z;
    double mcx = m * cx;
    double mcy = m * cy;
    double mcz = m * cz;
    if (M instanceof Matrix6dBase) {
        Matrix6dBase M6 = (Matrix6dBase) M;
        M6.m00 += m;
        M6.m11 += m;
        M6.m22 += m;
        M6.m33 += mcy * cy + mcz * cz;
        M6.m44 += mcx * cx + mcz * cz;
        M6.m55 += mcx * cx + mcy * cy;
        M6.m34 -= mcx * cy;
        M6.m35 -= mcx * cz;
        M6.m45 -= mcy * cz;
        M6.m43 -= mcx * cy;
        M6.m53 -= mcx * cz;
        M6.m54 -= mcy * cz;
        M6.m04 += mcz;
        M6.m05 -= mcy;
        M6.m15 += mcx;
        M6.m13 -= mcz;
        M6.m23 += mcy;
        M6.m24 -= mcx;
        M6.m40 += mcz;
        M6.m50 -= mcy;
        M6.m51 += mcx;
        M6.m31 -= mcz;
        M6.m32 += mcy;
        M6.m42 -= mcx;
    } else if (M instanceof MatrixNd) {
        MatrixNd MN = (MatrixNd) M;
        if (MN.rowSize() < 6 || M.colSize() < 6) {
            throw new IllegalArgumentException("M must be a Matrix6dBase or a MatrixNd with size at least 6x6");
        }
        MN.add(0, 0, m);
        MN.add(1, 1, m);
        MN.add(2, 2, m);
        MN.add(3, 3, mcy * cy + mcz * cz);
        MN.add(4, 4, mcx * cx + mcz * cz);
        MN.add(5, 5, mcx * cx + mcy * cy);
        MN.add(3, 4, -mcx * cy);
        MN.add(3, 5, -mcx * cz);
        MN.add(4, 5, -mcy * cz);
        MN.add(4, 3, -mcx * cy);
        MN.add(5, 3, -mcx * cz);
        MN.add(5, 4, -mcy * cz);
        MN.add(0, 4, mcz);
        MN.add(0, 5, -mcy);
        MN.add(1, 5, mcx);
        MN.add(1, 3, -mcz);
        MN.add(2, 3, mcy);
        MN.add(2, 4, -mcx);
        MN.add(4, 0, mcz);
        MN.add(5, 0, -mcy);
        MN.add(5, 1, mcx);
        MN.add(3, 1, -mcz);
        MN.add(3, 2, mcy);
        MN.add(4, 2, -mcx);
    } else {
        throw new IllegalArgumentException("M must be a Matrix6dBase or a MatrixNd with size at least 6x6");
    }
    if (M instanceof SpatialInertia) {
        ((SpatialInertia) M).componentUpdateNeeded = true;
    }
}