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

Example 36 with VectorNd

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

the class MotionTargetTerm method getTargetWeights.

/**
 * Returns target weights, one per target point.  Used by properties so can be get/set
 * through interface
 */
public VectorNd getTargetWeights() {
    VectorNd out = new VectorNd(mySources.size());
    getTargetWeights(out);
    return out;
}

Example 37 with VectorNd

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

the class TrackingController method setMotionTargetWeight.

public void setMotionTargetWeight(MotionTargetComponent comp, double w) {
    if (myMotionTerm == null) {
        return;
    }
    VectorNd wvec = myMotionTerm.getTargetWeights();
    int idx = myMotionTerm.getTargets().indexOf(comp);
    if (idx == -1) {
        idx = myMotionTerm.getSources().indexOf(comp);
    }
    if (idx != -1 && idx < wvec.size()) {
        wvec.set(idx, w);
        myMotionTerm.setTargetWeights(wvec);
    }
}

Example 38 with VectorNd

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

the class AnisotropicLinearMaterial method toRowMajor.

private static VectorNd toRowMajor(DenseMatrix A) {
    VectorNd vec = new VectorNd(A.rowSize() * A.colSize());
    int idx = 0;
    for (int i = 0; i < A.rowSize(); ++i) {
        for (int j = 0; j < A.colSize(); ++j) {
            vec.set(idx++, A.get(i, j));
        }
    }
    return vec;
}

Example 39 with VectorNd

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

the class MechSystemSolver method projectSingleFrictionConstraint.

private double projectSingleFrictionConstraint(VectorNd vel, SparseBlockMatrix DT, int bj, double phiMax, double doff, boolean ignoreRigidBodies) {
    int nactive = mySys.numActiveComponents();
    int nparam = mySys.numParametricComponents();
    int nlimit = nactive;
    if (MechSystemBase.myParametricsInSystemMatrix) {
        nlimit += nparam;
    }
    Vector3d d = new Vector3d();
    Vector3d r = new Vector3d();
    double[] vbuf = vel.getBuffer();
    double[] pbuf = myUpar.getBuffer();
    double[] vtbuf = new double[1];
    double phi;
    double dmd = 0;
    for (MatrixBlock blk = DT.firstBlockInCol(bj); blk != null; blk = blk.down()) {
        int bi = blk.getBlockRow();
        if (bi < nactive) {
            blk.mulTransposeAdd(vtbuf, 0, vbuf, DT.getBlockRowOffset(bi));
            if (blk instanceof Matrix3x1) {
                ((Matrix3x1) blk).get(d);
                // XXX assumes that corresponding block is Matrix3d
                Matrix3dBase Minv = (Matrix3dBase) myInverseMass.getBlock(bi, bi);
                Minv.mul(r, d);
                dmd += r.dot(d);
            } else if (!ignoreRigidBodies) {
            // XXX implement
            }
        } else if (MechSystemBase.myParametricsInSystemMatrix && bi < nactive + nparam) {
            int poff = DT.getBlockRowOffset(bi) - myActiveVelSize;
            blk.mulTransposeAdd(vtbuf, 0, pbuf, poff);
        }
    }
    double vt = vtbuf[0] - doff;
    // prevent division by zero
    if (dmd == 0) {
        dmd = 1e-16;
    }
    // System.out.println (" vtMag=" + vt + " dmd=" + dmd + " phiMax="+phiMax);
    if (vt > dmd * phiMax) {
        phi = -phiMax;
    } else if (vt < -dmd * phiMax) {
        phi = phiMax;
    } else {
        phi = -vt / dmd;
    }
    VectorNd dvec = new VectorNd();
    for (MatrixBlock blk = DT.firstBlockInCol(bj); blk != null; blk = blk.down()) {
        int bi = blk.getBlockRow();
        if (bi >= nactive) {
            break;
        }
        if (blk.rowSize() == 3) {
            dvec.setSize(3);
            blk.getColumn(0, dvec);
            MatrixBlock Minv = myInverseMass.getBlock(bi, bi);
            dvec.scale(phi);
            Minv.mulAdd(vbuf, DT.getBlockRowOffset(bi), dvec.getBuffer(), 0);
        } else if (!ignoreRigidBodies) {
        // XXX implement
        }
    }
    return phi;
}

Example 40 with VectorNd

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

the class MechSystemSolver method preRotate.

private VectorNd preRotate(VectorNd vec, RotationMatrix3d[] R) {
    int nbr = R.length;
    VectorNd vr = new VectorNd(vec.size());
    Vector3d v3 = new Vector3d();
    for (int bi = 0; bi < nbr; bi++) {
        v3.x = vec.get(bi * 3 + 0);
        v3.y = vec.get(bi * 3 + 1);
        v3.z = vec.get(bi * 3 + 2);
        v3.transform(R[bi]);
        vr.set(bi * 3 + 0, v3.x);
        vr.set(bi * 3 + 1, v3.y);
        vr.set(bi * 3 + 2, v3.z);
    }
    return vr;
}