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

Example 6 with MatrixNd

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

the class ForceTargetTerm method getTerm.

/**
 * Fills <code>H</code> and <code>b</code> with this motion term
 * @param H LHS matrix to fill
 * @param b RHS vector to fill
 * @param rowoff row offset to start filling term
 * @param t0 starting time of time step
 * @param t1 ending time of time step
 * @return next row offset
 */
public int getTerm(MatrixNd H, VectorNd b, int rowoff, double t0, double t1) {
    double h = TimeBase.round(t1 - t0);
    // System.out.println("using pre-comp data");
    // set myTargetForce
    updateTargetForce(t0, t1);
    VectorNd cbar = new VectorNd(myTargetForSize);
    // XXX do useTimestepScaling, useNormalizeH on targetVel
    // cbar.sub (myTargetFor, myController.getData ().getC0 ());
    // scale cbar by h -- Benedikt
    cbar.set(myTargetFor);
    cbar.scale(h);
    // XXX do useTimestepScaling, useNormalizeH on targetVel
    cbar.sub(myController.getData().getC0());
    MatrixNd Hc = new MatrixNd(myTargetForSize, myController.numExcitations());
    Hc.set(myController.getData().getHc());
    if (myController.getData().normalizeH) {
        double fn = 1.0 / Hc.frobeniusNorm();
        Hc.scale(fn);
        cbar.scale(fn);
    }
    if (myController.getData().useTimestepScaling) {
        // makes it independent of the time step
        Hc.scale(1 / h);
        cbar.scale(1 / h);
    }
    // apply weights
    if (myTargetWgts != null) {
        MotionForceInverseData.diagMul(myTargetWgts, Hc, Hc);
        MotionForceInverseData.pointMul(myTargetWgts, cbar, cbar);
    }
    if (myWeight >= 0) {
        Hc.scale(myWeight);
        cbar.scale(myWeight);
    }
    H.setSubMatrix(rowoff, 0, Hc);
    b.setSubVector(rowoff, cbar);
    /* debug output */
    if (myController.getDebug()) {
    // System.out.println ("(ForceTargetTerm)");
    // System.out.println("\tTargetForce: " + myTargetFor);
    // System.out.println("\tJc:\n" + getForceJacobian ().toString ("%.3f"));
    // System.out.println("\tHc:\n" + Hc.toString ("%.3f"));
    // System.out.println("\tcbar: " + cbar.toString ("%.3f"));
    }
    return rowoff + Hc.rowSize();
}

Example 7 with MatrixNd

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

the class L2RegularizationTerm method compute.

@Override
protected void compute(double t0, double t1) {
    int size = H.rowSize();
    H.setIdentity();
    H.scale(Math.sqrt(myWeight));
    if (weights != null && weights.size() != size) {
        // XXX doesn't seem like the best way to go about this
        System.out.println("Weights and term size mismatched.");
        weights = null;
    }
    // if null weights, do nothing, as though W == Identity
    if (weights != null) {
        if (W == null) {
            W = new MatrixNd(size, size);
        }
        /* make a diagonal matrix from the weights and multiply H by it */
        W.setZero();
        for (int i = 0; i < size; i++) {
            W.set(i, i, Math.sqrt(weights.get(i)));
        }
        H.mul(W);
    }
// if (TrackingController.isDebugTimestep (t0, t1)) {
// System.out.println("dt = " + dt + "    |Hl2| = " + H.frobeniusNorm());
// }
}

Example 8 with MatrixNd

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

the class MotionTargetTerm method getTerm.

/**
 * Fills <code>H</code> and <code>b</code> with this motion term
 * @param H LHS matrix to fill
 * @param b RHS vector to fill
 * @param rowoff row offset to start filling term
 * @param t0 starting time of time step
 * @param t1 ending time of time step
 * @return next row offset
 */
public int getTerm(MatrixNd H, VectorNd b, int rowoff, double t0, double t1) {
    double h = TimeBase.round(t1 - t0);
    // set myTargetVel
    updateTarget(t0, t1);
    // set myCurrentVel
    updateModelVelocity();
    // fixTargetPositions();   // XXX not sure why this is needed
    VectorNd vbar = new VectorNd(myTargetVelSize);
    // XXX do useTimestepScaling, useNormalizeH on targetVel
    vbar.sub(myTargetVel, myController.getData().getV0());
    if (myController.getDebug()) {
        System.out.println("(MotionTargetTerm)");
        System.out.println("\tmyTargetVel: " + myTargetVel.toString("%.3f"));
        System.out.println("\tV0: " + myController.getData().getV0().toString("%.3f"));
        System.out.println("\tvbar: " + vbar.toString("%.3f"));
    }
    MatrixNd Hv = new MatrixNd(myTargetVelSize, myController.numExcitations());
    Hv.set(myController.getData().getHv());
    if (myController.getData().normalizeH) {
        double fn = 1.0 / Hv.frobeniusNorm();
        Hv.scale(fn);
        vbar.scale(fn);
    }
    if (myController.getData().useTimestepScaling) {
        // makes it independent of the time step
        Hv.scale(1 / h);
        vbar.scale(1 / h);
    }
    // apply weights
    if (myTargetWgts != null) {
        MotionForceInverseData.diagMul(myTargetWgts, Hv, Hv);
        MotionForceInverseData.pointMul(myTargetWgts, vbar, vbar);
    }
    if (myWeight >= 0) {
        Hv.scale(myWeight);
        vbar.scale(myWeight);
    }
    H.setSubMatrix(rowoff, 0, Hv);
    b.setSubVector(rowoff, vbar);
    return rowoff + Hv.rowSize();
// return myMotionTerm.getTerm(H, b, rowoff, t0, t1,
// myTargetVel, myCurrentVel, myTargetWgts, getVelocityJacobian());
}

Example 9 with MatrixNd

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

the class SphericalJointForceBound method addHalfspaceBound.

/**
 * Add a half-space bound to the spherical joint
 * This vector is specified relative to a global coordinate frame
 * @param n vector specifying the half-space bound
 */
public void addHalfspaceBound(Vector3d n) {
    bounds.add(n);
    N = new MatrixNd(bounds.size(), 3);
    for (int i = 0; i < bounds.size(); ++i) {
        N.setRow(i, bounds.get(i));
    }
    H.setSize(bounds.size(), 3);
    // represent the vector in frame coordinates
    globalToFrame();
}

Example 10 with MatrixNd

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

the class MechSystemSolver method printSysMatrix.

private void printSysMatrix(SparseBlockMatrix S, String fmt, int size) {
    MatrixNd SS = new MatrixNd(size, size);
    for (int i = 0; i < size; i++) {
        for (int j = 0; j < size; j++) {
            SS.set(i, j, S.get(i, j));
        }
    }
    System.out.println("S=[\n" + SS.toString("%12.7f"));
    System.out.println("]");
}