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

Example 31 with VectorNd

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

the class DynamicRegularizationTerm method applyMapping.

/*
    * Apply a monotonic decreasing function that maps any real value to [0,1]
    * Returns a set of weights that penalizes low activations 
    */
// TODO multiple maps: exponential (softmax), squared, higher powers?
private VectorNd applyMapping(VectorNd x) {
    double[] w = new double[x.size()];
    double sum = 0;
    int n = w.length;
    switch(mapping) {
        case EXPONENTIAL:
            // System.out.println("Applying exponential mapping");
            double min = x.minElement();
            for (int i = 0; i < n; i++) {
                // w(i) > 0 for numeric precision of exponential
                w[i] = x.get(i) - min;
                w[i] = Math.exp(-w[i] * param * 1e-6);
                sum += w[i];
            }
            break;
        case MONOMIAL:
            // System.out.println("Applying monomial mapping");
            double max = x.maxElement();
            for (int i = 0; i < n; i++) {
                w[i] = Math.pow(-x.get(i) + max, param);
                sum += w[i];
            }
            break;
        case SIGMOID:
            // System.out.println("Applying sigmoid mapping");
            VectorNd sorted = new VectorNd();
            sorted.sort(x);
            /*
             * This part is a little hacky.. trying to position
             * the transition point of the sigmoid function at
             * some sort of median location. 
             */
            double l = sorted.get((int) Math.floor(sigmoidMean * (n - 1)));
            double h = sorted.get((int) Math.ceil(sigmoidMean * (n - 1)));
            double frac = sigmoidMean * (n - 1) - Math.floor(sigmoidMean * (n - 1));
            double mean = l + frac * (h - l);
            for (int i = 0; i < n; i++) {
                w[i] = 1 / (1 + Math.exp(param * (x.get(i) - mean)));
                sum += w[i];
            }
    }
    if (isNormalized) {
        for (int i = 0; i < n; i++) {
            w[i] *= n / sum;
        }
    }
    return new VectorNd(w);
}

Example 32 with VectorNd

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

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

the class MotionTargetTerm method updateModelVelocity.

private void updateModelVelocity() {
    int n = myMech.getActiveVelStateSize();
    if (myCurrentVel == null || myCurrentVel.size() != n)
        myCurrentVel = new VectorNd(n);
    myMech.getActiveVelState(myCurrentVel);
}

Example 34 with VectorNd

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

the class MotionTargetTerm method interpolateTargetVelocity.

/*
    * calculate target velocity by differencing current and last target positions
    */
private void interpolateTargetVelocity(double h) {
    // paranoid
    assert myTargets.size() == mySources.size();
    updateTargetPos(h);
    if (prevTargetPos == null || prevTargetPos.size() != myTargetPosSize) {
        prevTargetPos = new VectorNd(myTargetPos);
    }
    double[] prevPosBuf = prevTargetPos.getBuffer();
    int idx = 0;
    for (int i = 0; i < myTargets.size(); i++) {
        MotionTargetComponent target = myTargets.get(i);
        if (target instanceof Point) {
            idx = tmpPoint.setPosState(prevPosBuf, idx);
            interpolateTargetVelocityFromPositions(tmpPoint, (Point) target, h);
        } else if (target instanceof Frame) {
            idx = tmpFrame.setPosState(prevPosBuf, idx);
            interpolateTargetVelocityFromPositions(tmpFrame, (Frame) target, h);
        }
    }
    prevTargetPos.set(myTargetPos);
}

Example 35 with VectorNd

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

the class MotionTargetTerm method updateTargetVel.

private void updateTargetVel(double h) {
    if (myTargetVel == null || myTargetVel.size() != myTargetVelSize)
        myTargetVel = new VectorNd(myTargetVelSize);
    double[] velBuf = myTargetVel.getBuffer();
    int idx = 0;
    for (int i = 0; i < myTargets.size(); i++) {
        idx = myTargets.get(i).getVelState(velBuf, idx);
    }
}