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

Example 26 with VectorNd

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

the class ForceTargetTerm method updateWeightsVector.

private void updateWeightsVector() {
    myTargetWgts = new VectorNd(myForceTargets.size());
    int idx = 0;
    for (int t = 0; t < myForceTargets.size(); t++) {
        double w = myTargetWeights.get(t);
        int targetSize = myForceTargets.get(t).getTargetLambda().size();
        for (int i = 0; i < targetSize; i++) {
            myTargetWgts.set(idx++, w);
        }
    }
}

Example 27 with VectorNd

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

the class ForceTargetTerm method updateTargetForce.

public void updateTargetForce(double t0, double t1) {
    if (!isEnabled()) {
        return;
    }
    if (myTargetFor == null || myTargetFor.size() != myTargetForSize)
        myTargetFor = new VectorNd(myTargetForSize);
    double[] buf = myTargetFor.getBuffer();
    int idx = 0;
    for (int i = 0; i < myForceTargets.size(); i++) {
        ForceTarget target = myForceTargets.get(i);
        VectorNd lambda = target.getTargetLambda();
        idx = lambda.get(buf, idx);
    }
// System.out.println("targetForce = "+myTargetFor);
}

Example 28 with VectorNd

use of maspack.matrix.VectorNd 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 29 with VectorNd

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

the class ForceTargetTerm method createForceJacobian.

private void createForceJacobian() {
    MechModel mechMod = (MechModel) myMech;
    SparseBlockMatrix GT = new SparseBlockMatrix();
    VectorNd dg = new VectorNd();
    mechMod.getBilateralConstraints(GT, dg);
    if (debug) {
        System.out.println("num con = " + mechMod.bodyConnectors().size());
        System.out.println(GT.colSize());
        System.out.println(GT.rowSize());
        System.out.println(GT.getSize());
        System.out.println(GT.numBlocks());
        System.out.println(GT.getBlock(0, 0));
        System.out.println(GT.getBlock(0, 1));
        System.out.println(GT.getBlock(0, 2));
        System.out.println(GT.getBlock(1, 0));
    }
    // find the number of bilateral constraints for each connector
    int[] connectorSizes = new int[mechMod.bodyConnectors().size()];
    int[] targetToConnectorMap = new int[myForceTargets.size()];
    int targetIdx = 0;
    for (ForceTarget ft : myForceTargets) {
        int connectorIdx = 0;
        for (BodyConnector connector : mechMod.bodyConnectors()) {
            if (debug) {
                System.out.println(connector.getName());
                System.out.println(ft.getName());
            }
            if (ft.getConnector() == connector) {
                targetToConnectorMap[targetIdx] = connectorIdx;
                targetIdx++;
            }
            if (connector.isEnabled() == true) {
                if (debug) {
                    System.out.println(connector.numBilateralConstraints());
                }
                connectorSizes[connectorIdx] = connector.numBilateralConstraints();
                connectorIdx++;
            }
        }
    }
    myForJacobian = new SparseBlockMatrix(new int[0], connectorSizes);
    for (int i = 0; i < myForceTargets.size(); i++) {
        ForceTarget target = myForceTargets.get(i);
        // TODO: non-enabled connectors should not add to Jacobian -- need to fix
        target.addForceJacobian(myForJacobian, i, targetToConnectorMap[i]);
    }
    if (debug) {
        System.out.println("Jc = " + myForJacobian);
    }
}

Example 30 with VectorNd

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

the class LeastSquaresSolver method resetBounds.

private int resetBounds() {
    int size = 0;
    if (myLowerBound != null) {
        lb = new VectorNd(exSize);
        for (int i = 0; i < exSize; i++) {
            lb.set(i, myLowerBound);
        }
        size += exSize;
    } else {
        lb = null;
    }
    if (myUpperBound != null) {
        ub = new VectorNd(exSize);
        for (int i = 0; i < exSize; i++) {
            ub.set(i, myUpperBound);
        }
        size += exSize;
    } else {
        ub = null;
    }
    return size;
}