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

Example 51 with Matrix3d

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

the class DistanceGridSurfCalc method createTetBarycentricMats.

private void createTetBarycentricMats() {
    Matrix3d[] mats = new Matrix3d[6];
    Vector3d v = new Vector3d();
    for (TetID id : TetID.values()) {
        Matrix3d M = new Matrix3d();
        int[] nodeNums = id.getNodes();
        // base node is known to be at zero
        for (int j = 0; j < 3; j++) {
            v.set(DistanceGrid.myBaseQuadCellXyzi[nodeNums[j + 1]]);
            v.scale(0.5);
            M.setColumn(j, v);
        }
        M.invert();
        mats[id.intValue()] = M;
    }
    myTetBarycentricMats = mats;
}

Example 52 with Matrix3d

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

the class CPD method rigid.

/**
 * Uses the rigid CPD algorithm to align a set of points
 * @param X reference input points
 * @param Y points to register
 * @param w weight, accounting to noise (w=0 --> no noise)
 * @param tol will iterative until objective function changes by less than this
 * @param maxIters maximum number of iterations
 * @param allowScaling whether or not to allow scaling
 * @param TY transformed points
 * @param trans initial guess of scaled rigid transform
 * @param sigma2Holder initial guess of variance
 * @return the scaled rigid transform for registration
 */
public static ScaledRigidTransform3d rigid(Point3d[] X, Point3d[] Y, double w, double tol, int maxIters, boolean allowScaling, Point3d[] TY, ScaledRigidTransform3d trans, double[] sigma2Holder) {
    int M = Y.length;
    int N = X.length;
    if (trans == null) {
        trans = new ScaledRigidTransform3d();
        transformPoints(Y, TY);
    } else {
        transformPoints(Y, trans, TY);
    }
    double sigma2;
    if (sigma2Holder == null || sigma2Holder[0] < 0) {
        sigma2 = computeVariance(X, TY, null, 1.0 / M);
    } else {
        sigma2 = sigma2Holder[0];
    }
    SVDecomposition3d svd = new SVDecomposition3d();
    Matrix3d R = new Matrix3d(trans.R);
    Vector3d t = new Vector3d(trans.p);
    double s = trans.s;
    double[][] P = new double[M][N];
    double[] P1 = new double[M];
    double[] Pt1 = new double[N];
    double Np;
    double[] tr = new double[2];
    Matrix3d A = new Matrix3d();
    Matrix3d UVt = new Matrix3d();
    Matrix3d C = new Matrix3d();
    C.set(0, 0, 1);
    C.set(1, 1, 1);
    Point3d meanx = new Point3d();
    Point3d meany = new Point3d();
    double err = Double.MAX_VALUE;
    int iters = 0;
    double q, qprev;
    q = Double.MAX_VALUE;
    // iterative part of algorithm
    while ((iters < maxIters) && (err > tol)) {
        // E-step
        Np = computeP(X, TY, sigma2, w, P, P1, Pt1);
        // M-step
        // mean
        computeMean(X, Pt1, Np, meanx);
        computeMean(Y, P1, Np, meany);
        // A = (X-mean(X))'*P'*(Y-mean(Y))
        // d = trace( trace(Y'*diag(P1)*Y) );
        computeAD(X, meanx, P, P1, Pt1, Y, meany, A, tr);
        // R = U*C*V', C= diag([1 1 det(U*V')])
        svd.factor(A);
        UVt.set(svd.getU());
        UVt.mulTranspose(svd.getV());
        C.set(2, 2, UVt.determinant());
        R.set(svd.getU());
        R.mul(C);
        R.mulTranspose(svd.getV());
        // s = trace(A'*R)/trace(Y'*diag(P1)*Y)
        A.mulTransposeLeft(A, R);
        double trAtR = A.trace();
        if (allowScaling) {
            s = trAtR / tr[1];
        }
        // t = mean(X)-s*R*mean(Y)
        t.mul(R, meany);
        t.scale(-s);
        t.add(meanx);
        // Adjust output
        transformPoints(Y, s, R, t, TY);
        // New objective function
        qprev = q;
        // q = computeQ(X, TY, P, Np, sigma2);
        q = (tr[0] - 2 * s * trAtR + s * s * tr[1]) / (2 * sigma2) + 1.5 * Np * Math.log(sigma2);
        // new variance estimate
        // sigma2 = computeVariance(X, TY, P, Np);
        sigma2 = (tr[0] - s * trAtR) / (3 * Np);
        if (sigma2 <= 0) {
            sigma2 = tol;
        }
        err = Math.abs(q - qprev);
        iters++;
    }
    if (verbose) {
        System.out.println("Registration complete in " + iters + " iterations");
    }
    // copy info over
    trans.R.set(R);
    trans.p.set(t);
    // triggers update of internal matrix
    trans.setScale(s);
    if (sigma2Holder != null) {
        sigma2Holder[0] = sigma2;
    }
    return trans;
}

Example 53 with Matrix3d

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

the class CPD method affine.

/**
 * Uses the affine CPD algorithm to align a set of points
 * @param X reference input points
 * @param Y points to register
 * @param w weight, accounting to noise (w=0 --> no noise)
 * @param tol will iterative until objective function changes by less than this
 * @param maxIters maximum number of iterations
 * @param TY transformed points
 * @param trans initial guess of scaled rigid transform
 * @param sigma2Holder initial guess of variance
 * @return the scaled rigid transform for registration
 */
public static AffineTransform3d affine(Point3d[] X, Point3d[] Y, double w, double tol, int maxIters, Point3d[] TY, AffineTransform3d trans, double[] sigma2Holder) {
    int M = Y.length;
    int N = X.length;
    SVDecomposition3d svd = new SVDecomposition3d();
    if (trans == null) {
        trans = new AffineTransform3d();
        transformPoints(Y, TY);
    } else {
        transformPoints(Y, trans, TY);
    }
    double sigma2;
    if (sigma2Holder == null || sigma2Holder[0] < 0) {
        sigma2 = computeVariance(X, TY, null, 1.0 / M);
    } else {
        sigma2 = sigma2Holder[0];
    }
    Matrix3d B = new Matrix3d(trans.A);
    Vector3d t = new Vector3d(trans.p);
    double[][] P = new double[M][N];
    double[] P1 = new double[M];
    double[] Pt1 = new double[N];
    double Np;
    Matrix3d A = new Matrix3d();
    Matrix3d D = new Matrix3d();
    Matrix3d YPY = new Matrix3d();
    double[] tr = new double[2];
    Point3d meanx = new Point3d();
    Point3d meany = new Point3d();
    double err = Double.MAX_VALUE;
    int iters = 0;
    double q, qprev;
    q = Double.MAX_VALUE;
    // iterative part of algorithm
    while ((iters < maxIters) && (err > tol)) {
        // E-step
        Np = computeP(X, TY, sigma2, w, P, P1, Pt1);
        // M-step
        // mean
        computeMean(X, Pt1, Np, meanx);
        computeMean(Y, P1, Np, meany);
        // A = (X-mean(X))'*P'*(Y-mean(Y))
        // D = (Y-mean(Y))'*diag(P1)*(Y-mean(Y))
        computeAD(X, meanx, P, P1, Pt1, Y, meany, A, YPY, tr);
        // B = A*inverse(D)
        svd.factor(YPY);
        // pseudo-invert
        svd.pseudoInverse(D);
        B.mul(A, D);
        // t = mean(X)-A*mean(Y)
        t.mul(B, meany);
        t.sub(meanx, t);
        // Adjust output
        transformPoints(Y, B, t, TY);
        // speedy compute Q and sigma2
        A.mulTranspose(B);
        double trABt = A.trace();
        YPY.mulTranspose(B);
        YPY.mul(B);
        double trBYPYB = YPY.trace();
        // compute new objective (speedy)
        qprev = q;
        // q = computeQ(X, TY, P, Np, sigma2);
        q = (tr[0] - 2 * trABt + trBYPYB) / (2 * sigma2) + 1.5 * Np * Math.log(sigma2);
        // new variance estimate (speedy)
        // sigma2 = computeVariance(X, TY, P, Np);
        sigma2 = (tr[0] - trABt) / (3 * Np);
        if (sigma2 <= 0) {
            sigma2 = tol;
        }
        err = Math.abs(q - qprev);
        iters++;
    }
    if (verbose) {
        System.out.println("Registration complete in " + iters + " iterations");
    }
    // copy info over
    trans.A.set(B);
    trans.p.set(t);
    if (sigma2Holder != null) {
        sigma2Holder[0] = sigma2;
    }
    return trans;
}

Example 54 with Matrix3d

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

the class CubicHyperelastic method main.

public static void main(String[] args) {
    CubicHyperelastic mat = new CubicHyperelastic();
    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.setG10(1.2);
    mat.setG30(3.5);
    mat.setG20(1.9);
    mat.setBulkModulus(0.0);
    mat.computeStress(sig, def, Q, null);
    // pt.setStress (sig);
    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 55 with Matrix3d

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

the class FungMaterial method computeStress.

public void computeStress(SymmetricMatrix3d sigma, SolidDeformation def, Matrix3d Q, FemMaterial baseMat) {
    sigma.setZero();
    double[] K = new double[3];
    double[] L = new double[3];
    Vector3d[] a0 = { new Vector3d(), new Vector3d(), new Vector3d() };
    Vector3d[] a = { new Vector3d(), new Vector3d(), new Vector3d() };
    Vector3d vtmp = new Vector3d();
    // Matrix3d Q = Matrix3d.IDENTITY;
    // if (dt.getFrame() != null) {
    // Q = dt.getFrame();
    // }
    SymmetricMatrix3d[] A = { new SymmetricMatrix3d(), new SymmetricMatrix3d(), new SymmetricMatrix3d() };
    Matrix3d tmpMatrix = new Matrix3d();
    Matrix3d tmpMatrix2 = new Matrix3d();
    SymmetricMatrix3d tmpSymmMatrix = new SymmetricMatrix3d();
    SymmetricMatrix3d sigmaFung = new SymmetricMatrix3d();
    // Evaluate Lame coefficients
    mu[0] = myMU1;
    mu[1] = myMU2;
    mu[2] = myMU3;
    lam[0][0] = myL11;
    lam[0][1] = myL12;
    lam[0][2] = myL31;
    lam[1][0] = myL12;
    lam[1][1] = myL22;
    lam[1][2] = myL23;
    lam[2][0] = myL31;
    lam[2][1] = myL23;
    lam[2][2] = myL33;
    double J = def.getDetF();
    double avgp = def.getAveragePressure();
    // Calculate deviatoric left Cauchy-Green tensor
    def.computeDevLeftCauchyGreen(myB);
    // Calculate deviatoric right Cauchy-Green tensor
    def.computeDevRightCauchyGreen(myC);
    // calculate square of C
    myC2.mulTransposeLeft(myC);
    Matrix3d mydevF = new Matrix3d(def.getF());
    mydevF.scale(Math.pow(J, -1.0 / 3.0));
    for (int i = 0; i < 3; i++) {
        // Copy the texture direction in the reference configuration to a0
        a0[i].x = Q.get(0, i);
        a0[i].y = Q.get(1, i);
        a0[i].z = Q.get(2, i);
        vtmp.mul(myC, a0[i]);
        K[i] = a0[i].dot(vtmp);
        vtmp.mul(myC2, a0[i]);
        L[i] = a0[i].dot(vtmp);
        a[i].mul(mydevF, a0[i]);
        a[i].scale(Math.pow(K[i], -0.5));
        A[i].set(a[i].x * a[i].x, a[i].y * a[i].y, a[i].z * a[i].z, a[i].x * a[i].y, a[i].x * a[i].z, a[i].y * a[i].z);
    }
    // Evaluate exp(Q)
    double eQ = 0.0;
    for (int i = 0; i < 3; i++) {
        eQ += 2.0 * mu[i] * (L[i] - 2.0 * K[i] + 1.0);
        for (int j = 0; j < 3; j++) eQ += lam[i][j] * (K[i] - 1.0) * (K[j] - 1.0);
    }
    eQ = Math.exp(eQ / (4. * myCC));
    // Evaluate the stress
    SymmetricMatrix3d bmi = new SymmetricMatrix3d();
    bmi.sub(myB, SymmetricMatrix3d.IDENTITY);
    for (int i = 0; i < 3; i++) {
        // s += mu[i]*K[i]*(A[i]*bmi + bmi*A[i]);
        tmpMatrix.mul(A[i], bmi);
        tmpMatrix2.mul(bmi, A[i]);
        tmpMatrix.add(tmpMatrix2);
        tmpMatrix.scale(mu[i] * K[i]);
        tmpSymmMatrix.setSymmetric(tmpMatrix);
        sigmaFung.add(tmpSymmMatrix);
        for (int j = 0; j < 3; j++) {
            // s += lam[i][j]*((K[i]-1)*K[j]*A[j]+(K[j]-1)*K[i]*A[i])/2.;
            sigmaFung.scaledAdd(lam[i][j] / 2.0 * (K[i] - 1.0) * K[j], A[j]);
            sigmaFung.scaledAdd(lam[i][j] / 2.0 * (K[j] - 1.0) * K[i], A[i]);
        }
    }
    sigmaFung.scale(eQ / (2.0 * J));
    sigmaFung.deviator();
    sigmaFung.m10 = sigmaFung.m01;
    sigmaFung.m20 = sigmaFung.m02;
    sigmaFung.m21 = sigmaFung.m12;
    sigma.add(sigmaFung);
    sigma.m00 += avgp;
    sigma.m11 += avgp;
    sigma.m22 += avgp;
}