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

Example 76 with VectorNd

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

the class AxisAngleField method textToValue.

public Object textToValue(String text, BooleanHolder corrected, StringHolder errMsg) {
    corrected.value = false;
    ReaderTokenizer rtok = new ReaderTokenizer(new StringReader(text));
    VectorNd tmp = new VectorNd(4);
    try {
        for (int i = 0; i < 4; i++) {
            if (rtok.nextToken() != ReaderTokenizer.TT_NUMBER) {
                return illegalValue("Missing or malformed number for element " + i, errMsg);
            }
            tmp.set(i, rtok.nval);
        }
        if (rtok.nextToken() != ReaderTokenizer.TT_EOF) {
            // check if specified degrees or radians
            if (rtok.tokenIsWord()) {
                if (rtok.sval.toLowerCase().equals("r") || rtok.sval.toLowerCase().equals("rad") || rtok.sval.toLowerCase().equals("radians")) {
                    // radians
                    tmp.set(3, Math.toDegrees(tmp.get(3)));
                } else if (rtok.sval.toLowerCase().equals("d") || rtok.sval.toLowerCase().equals("deg") || rtok.sval.toLowerCase().equals("degrees")) {
                // already in degrees
                } else {
                    return illegalValue("Only angle unit specifier allowed after last entry", errMsg);
                }
            } else {
                return illegalValue("Extra characters after last element", errMsg);
            }
        }
        return validValue(new AxisAngle(tmp.get(0), tmp.get(1), tmp.get(2), Math.toRadians(tmp.get(3))), errMsg);
    } catch (Exception e) {
        return illegalValue("Improperly formed 4-tuple", errMsg);
    }
}

Example 77 with VectorNd

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

the class NumericInterval method validate.

/**
 * Validates a vector by checking that its elements lie within this range
 * interval. For purposes of this method, the interval is treated as closed.
 * If all elements are within range, the vector is returned unchanged.
 * Otherwise, the method returns either a new clipped vector (if
 * <code>clip</code> is <code>true</code>), or the special value
 * {@link Range#IllegalValue}. In
 * the latter two cases, an error message will also be returned if the
 * variable <code>errMsg</code> is non-null.
 *
 * @param vec
 * vector to validate
 * @param clip
 * if true, clip the vector to the range if possible
 * @param errMsg
 * if non-null, is used to return an error message if one or more elements
 * are out of range
 * @return either the original vector, a clipped version of it, or
 * Range.IllegalValue
 */
public Object validate(Vector vec, boolean clip, StringHolder errMsg) {
    boolean vectorCopied = false;
    for (int i = 0; i < vec.size(); i++) {
        double num = vec.get(i);
        if (!withinRange(num)) {
            if (clip && canClipToRange(num)) {
                if (!vectorCopied) {
                    vec = new VectorNd(vec);
                    vectorCopied = true;
                }
                vec.set(i, clipToRange(num));
            } else {
                setError(errMsg, "elements should be within the range " + this);
                return Range.IllegalValue;
            }
        }
    }
    if (vectorCopied) {
        setError(errMsg, "vector was clipped to range");
    }
    return vec;
}

Example 78 with VectorNd

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

the class SpatialInertiaTest method test.

public void test() throws Exception {
    SpatialInertia M1 = new SpatialInertia();
    SpatialInertia M2 = new SpatialInertia();
    SpatialInertia M3 = new SpatialInertia();
    double mass;
    Point3d com = new Point3d();
    SymmetricMatrix3d J = new SymmetricMatrix3d();
    Matrix3d F = new Matrix3d();
    for (int i = 0; i < 100; i++) {
        M1.setRandom(-0.5, 0.5, randGen);
        String s = M1.toString();
        M2.scan(new ReaderTokenizer(new StringReader(s)));
        checkEqual("toString/scan MASS_INERTIA check", M2, M1, EPS);
        s = M1.toString("%g", SpatialInertia.MATRIX_STRING);
        M2.scan(new ReaderTokenizer(new StringReader(s)));
        checkEqual("toString/scan MATRIX_STRING check", M2, M1, EPS);
        MatrixNd MA = new MatrixNd(0, 0);
        MatrixNd MB = new MatrixNd(0, 0);
        MA.set(M1);
        M2.set(MA);
        mass = randGen.nextDouble();
        com.setRandom(-0.5, 0.5, randGen);
        F.setRandom(-0.5, 0.5, randGen);
        J.mulTransposeLeft(F);
        M1.setMass(mass);
        M1.setCenterOfMass(com);
        M1.setRotationalInertia(J);
        checkComponents("set component check", M1, mass, com, J);
        M2.set(M1);
        checkComponents("set check", M2, mass, com, J);
        M1.set(mass, J, com);
        checkComponents("set check", M1, mass, com, J);
        M1.set(mass, J);
        com.setZero();
        checkComponents("set check", M1, mass, com, J);
        M1.setRandom();
        MA.set(M1);
        MB.set(M2);
        // check addition
        M2.add(M1);
        MB.add(MA);
        M3.set(MB);
        checkEqual("add check", M2, M3, EPS);
        // check subtraction
        MB.sub(MA);
        M2.sub(M1);
        M3.set(MB);
        checkEqual("sub check", M2, M3, EPS);
        // check scaling
        MB.scale(3);
        M2.scale(3);
        M3.set(MB);
        checkEqual("scale check", M2, M3, EPS);
        MatrixNd invM2 = new MatrixNd(0, 0);
        MatrixNd invMB = new MatrixNd(0, 0);
        // check inversion
        MB.set(M2);
        M2.getInverse(invM2);
        invMB.invert(MB);
        double eps = invM2.frobeniusNorm() * EPS;
        checkEqual("inverse check", invM2, invMB, eps);
        Matrix6d invM6 = new Matrix6d();
        SpatialInertia.invert(invM6, M2);
        checkEqual("inverse check", invM6, invMB, eps);
        CholeskyDecomposition chol = new CholeskyDecomposition();
        chol.factor(MB);
        MatrixNd U = new MatrixNd(6, 6);
        MatrixNd L = new MatrixNd(6, 6);
        chol.get(L);
        U.transpose(L);
        MatrixNd invU = new MatrixNd(6, 6);
        MatrixNd invL = new MatrixNd(6, 6);
        invU.invert(U);
        invL.transpose(invU);
        VectorNd vec = new VectorNd(6);
        VectorNd res = new VectorNd(6);
        Twist tw1 = new Twist();
        Twist twr = new Twist();
        Twist twrCheck = new Twist();
        Wrench wr1 = new Wrench();
        Wrench wrr = new Wrench();
        Wrench wrrCheck = new Wrench();
        // check multiply by vector (twist)
        tw1.setRandom();
        M2.mul(wrr, tw1);
        vec.set(tw1);
        MB.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul check", wrr, wrrCheck, EPS);
        // check inverse multiply by vector (wrench)
        wr1.setRandom();
        M2.mulInverse(twr, wr1);
        vec.set(wr1);
        invMB.mul(res, vec);
        twrCheck.set(res);
        checkEqual("inverse mul check", twr, twrCheck, eps);
        // check inverse multiply by vector (twist, twist)
        wr1.setRandom();
        twr.set(wr1);
        M2.mulInverse(twr, twr);
        vec.set(wr1);
        invMB.mul(res, vec);
        twrCheck.set(res);
        checkEqual("inverse mul check", twr, twrCheck, eps);
        // check right factor multiply by vector
        tw1.setRandom();
        M2.mulRightFactor(twr, tw1);
        vec.set(tw1);
        U.mul(res, vec);
        twrCheck.set(res);
        checkEqual("mul right factor", twr, twrCheck, EPS);
        M2.mulRightFactor(tw1, tw1);
        checkEqual("mul right factor (twr == tw1)", tw1, twrCheck, EPS);
        // check right factor inverse multiply by vector
        tw1.setRandom();
        M2.mulRightFactorInverse(twr, tw1);
        vec.set(tw1);
        invU.mul(res, vec);
        twrCheck.set(res);
        checkEqual("mul right factor inverse", twr, twrCheck, eps);
        M2.mulRightFactorInverse(tw1, tw1);
        checkEqual("mul right factor inverse (twr == tw1)", tw1, twrCheck, eps);
        // check left factor multiply by vector
        wr1.setRandom();
        M2.mulLeftFactor(wrr, wr1);
        vec.set(wr1);
        L.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul left factor", wrr, wrrCheck, EPS);
        M2.mulLeftFactor(wr1, wr1);
        checkEqual("mul left factor (wrr == wr1)", wr1, wrrCheck, EPS);
        // check left factor inverse multiply by vector
        wr1.setRandom();
        M2.mulLeftFactorInverse(wrr, wr1);
        vec.set(wr1);
        invL.mul(res, vec);
        wrrCheck.set(res);
        checkEqual("mul left factor inverse", wrr, wrrCheck, eps);
        M2.mulLeftFactorInverse(wr1, wr1);
        checkEqual("mul left factor inverse (wrr == wr1)", wr1, wrrCheck, eps);
        // check addition of point mass
        Matrix6d M6 = new Matrix6d();
        SpatialInertia MCheck = new SpatialInertia();
        MCheck.set(M1);
        M6.set(M1);
        Point3d com1 = new Point3d();
        double mass1 = randGen.nextDouble();
        com1.setRandom(-0.5, 0.5, randGen);
        Point3d com2 = new Point3d();
        double mass2 = randGen.nextDouble();
        com2.setRandom(-0.5, 0.5, randGen);
        M3.setPointMass(mass1, com1);
        MCheck.add(M3);
        M3.setPointMass(mass2, com2);
        MCheck.add(M3);
        M1.addPointMass(mass1, com1);
        M1.addPointMass(mass2, com2);
        checkEqual("point mass addition", M1, MCheck, EPS);
        SpatialInertia.addPointMass(M6, mass1, com1);
        SpatialInertia.addPointMass(M6, mass2, com2);
        Matrix6d MM = new Matrix6d();
        MM.sub(MCheck, M6);
        checkEqual("point mass addition", M6, MCheck, EPS);
        // check rotation transforms
        RotationMatrix3d R = new RotationMatrix3d();
        SpatialInertia MSave = new SpatialInertia(M1);
        R.setRandom();
        M1.getRotated(M6, R);
        MCheck.set(M1);
        MCheck.transform(R);
        checkEqual("getRotated", M6, MCheck, EPS);
        MCheck.inverseTransform(R);
        checkEqual("inverseTransform", MCheck, MSave, EPS);
        R.transpose();
        M1.set(M6);
        M1.getRotated(M6, R);
        // should be back to original
        checkEqual("getRotated (back)", M6, MSave, EPS);
    }
// // test creation of inertia from a mesh
// double density = 20;
// double wx = 3.0;
// double wy = 1.0;
// double wz = 2.0;
// SpatialInertia boxInertia =
// SpatialInertia.createBoxInertia (20 * wx * wy * wz, wx, wy, wz);
// PolygonalMesh boxMesh = MeshFactory.createQuadBox (wx, wy, wz);
// SpatialInertia meshInertia =
// SpatialInertia.createClosedVolumeInertia (boxMesh, density);
// 
// checkMeshInertia (boxMesh, density, boxInertia);
// 
// SpatialInertia innerBoxInertia =
// SpatialInertia.createBoxInertia (1.0 * density, 1, 1, 1);
// boxInertia.setBox (20 * wx * wy * wz, wx, wy, wz);
// boxInertia.sub (innerBoxInertia);
// boxMesh = buildMeshWithHole (wx, wy, wz);
// 
// checkMeshInertia (boxMesh, density, boxInertia);
// 
// boxMesh.triangulate();
// checkMeshInertia (boxMesh, density, boxInertia);
}

Example 79 with VectorNd

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

the class BusyThread method main.

public static void main(String[] args) {
    Vector<SparseElement> sparseMatrix = new Vector<SparseElement>();
    int pardisoMatSize = 0, pardisoNumVals = 5;
    double[] ra = null;
    int[] ia = null;
    int[] ja = null;
    VectorNd bsol = new VectorNd(0);
    VectorNd vsol = new VectorNd(0);
    pardisoNumVals = importSparseMatrix("lib/Xmat325.txt", sparseMatrix, pardisoMatSize);
    pardisoMatSize = nDia;
    ia = new int[pardisoMatSize + 2];
    ra = new double[(int) (nonZeroVactor * pardisoNumVals)];
    ja = new int[(int) (nonZeroVactor * pardisoNumVals)];
    sym = true;
    pardisoNumVals = sparse_2_pardiso(sparseMatrix, pardisoNumVals, ra, ia, ja);
    // int size = pardisoNumVals;
    // System.out.println("[");
    // for (int i = 0; i < size; i++)
    // {
    // System.out.print(ra[i] +",");
    // if (i%10 == 9)
    // System.out.println();
    // }
    // System.out.println("]");
    PardisoSolver pardiso = new PardisoSolver();
    System.out.println("pardisoMatSize:" + pardisoMatSize);
    System.out.println("pardisoNumVals:" + pardisoNumVals);
    System.out.println("ra.length:" + ra.length);
    System.out.println("ia.length:" + ia.length);
    System.out.println("ja.length:" + ja.length);
    vsol.setSize(pardisoMatSize);
    bsol.setSize(pardisoMatSize);
    for (int i = 0; i < pardisoMatSize; i++) {
        bsol.set(i, i % 4);
    }
    pardiso.analyze(ra, ja, ia, pardisoMatSize, Matrix.SYMMETRIC);
    BusyThread busy = new BusyThread();
    busy.start();
    for (int i = 0; i < 1000000; i++) {
        System.out.println(i + " factoring ...");
        pardiso.factor(ra);
        System.out.println("solving ...");
        pardiso.solve(vsol.getBuffer(), bsol.getBuffer());
    }
}

Example 80 with VectorNd

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

the class IncompleteCholeskyDecompositionTest method main.

public static void main(String[] args) {
    SparseMatrixNd A = new SparseMatrixNd(1, 1);
    Random random = new Random(123);
    try {
        A.scan(new ReaderTokenizer(new FileReader("/ubc/ece/home/hct/other/elliote/A.matrix")));
        System.out.println("read matrix " + A.rowSize() + " " + A.colSize());
    } catch (IOException e) {
        System.out.println(e.getMessage());
        System.exit(0);
    }
    int n = A.rowSize();
    System.out.println("loaded matrix");
    // int n = 70;
    // A.setSize(n, n);
    // SVDecomposition svd;
    // do
    // {
    // A.setRandom(1, 2, n*2, random);
    // for(int i = 0; i < n; i++)
    // A.set(i, i, random.nextDouble() + 1);
    // 
    // svd = new SVDecomposition(A);
    // }
    // while(svd.determinant() < 1e-10);
    // A.mulTranspose(A);
    // System.out.println("created spd matrix");
    // waitforuser();
    IncompleteCholeskyDecomposition icd = new IncompleteCholeskyDecomposition();
    icd.factor(A, 0.01);
    if (icd.C.containsNaN()) {
        System.out.println("L contains NaN");
    }
    System.out.println("factored matrix");
    // waitforuser();
    SparseMatrixNd tmp = new SparseMatrixNd(n, n);
    tmp.mulTransposeRight(icd.C, icd.C);
    // System.out.println(new MatrixNd(tmp));
    tmp.sub(A);
    System.out.println("residual matrix one norm " + tmp.oneNorm());
    VectorNd x = new VectorNd(n);
    VectorNd b = new VectorNd(n);
    VectorNd xc = new VectorNd(n);
    // try
    // {
    // System.out.println("writing solve matrix and incomplete cholesky
    // decomposition");
    // 
    // PrintWriter fwr = new
    // PrintWriter("/ubc/ece/home/hct/other/elliote/A.matrix");
    // fwr.append("[ ");
    // A.write(fwr, new NumberFormat(), WriteFormat.Sparse);
    // fwr.append(" ]");
    // fwr.close();
    // 
    // fwr = new PrintWriter("/ubc/ece/home/hct/other/elliote/L.matrix");
    // fwr.append("[ ");
    // icd.L.write(fwr, new NumberFormat(), WriteFormat.Sparse);
    // fwr.append(" ]");
    // fwr.close();
    // 
    // System.out.println("finished writing");
    // }
    // catch(IOException e)
    // {
    // System.out.println(e.getMessage());
    // System.exit(0);
    // }
    // test backsolves
    System.out.println();
    CGSolver isolver = new CGSolver();
    DirectSolver dsolver = new UmfpackSolver();
    // dsolver.factor(A);
    b.setRandom(-1, 1, random);
    System.out.println("solving L * x = b");
    icd.solve(x, b);
    dsolver.analyzeAndFactor(icd.C);
    dsolver.solve(xc, b);
    System.out.println("b " + b);
    System.out.println("x " + x);
    System.out.println("xc " + xc);
    if (!x.epsilonEquals(xc, 1e-6)) {
        System.out.println("backsolve failed");
    }
    System.out.println();
    System.out.println("solving L' * x = b");
    icd.solveTranspose(x, b);
    tmp.transpose(icd.C);
    dsolver.analyzeAndFactor(tmp);
    dsolver.solve(xc, b);
    System.out.println("b " + b);
    System.out.println("x " + x);
    System.out.println("xc " + xc);
    if (!x.epsilonEquals(xc, 1e-6)) {
        System.out.println("backsolve failed");
    }
    // test upcg solver
    System.out.println();
    System.out.println("solving A * x = b");
    double tol = 1e-20;
    int maxit = 500;
    System.out.println("preconditioned solve untransformed");
    x.setZero();
    isolver.solve(x, A, b, tol, maxit, icd);
    System.out.println("iterations " + isolver.getNumIterations());
    System.out.println("b " + b);
    System.out.println("x " + x);
    System.out.println();
    System.out.println("preconditioned solve transformed");
    x.setZero();
    isolver.solveTransformed(x, A, b, tol, maxit, icd);
    System.out.println("iterations " + isolver.getNumIterations());
    System.out.println("b " + b);
    System.out.println("x " + x);
    System.out.println();
    System.out.println("unpreconditioned solve");
    x.setZero();
    isolver.solve(x, A, b, tol, maxit);
    System.out.println("iterations " + isolver.getNumIterations());
    System.out.println("b " + b);
    System.out.println("x " + x);
    System.out.println();
    System.out.println("direct solve");
    x.setZero();
    dsolver.analyzeAndFactor(A);
    dsolver.solve(x, b);
    System.out.println("b " + b);
    System.out.println("x " + x);
}