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

Example 41 with VectorNd

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

the class MechSystemSolver method projectPosConstraints.

public void projectPosConstraints(double t) {
    updateStateSizes();
    updateMassMatrix(t);
    VectorNd q = new VectorNd(myActivePosSize);
    VectorNd u = new VectorNd(myActiveVelSize);
    StepAdjustment stepAdjust = new StepAdjustment();
    mySys.updateConstraints(t, stepAdjust, /*flags=*/
    MechSystem.COMPUTE_CONTACTS);
    mySys.getActivePosState(q);
    computePosCorrections(q, u, t);
    mySys.setActivePosState(q);
    // mySys.updateConstraints (
    // t, stepAdjust, /*flags=*/MechSystem.UPDATE_CONTACTS);
    // need to force an update of the mass matrix, since a subsequent
    // call to updateMassMatrix(t) wouldn't work.
    updateMassMatrix(-1);
}

Example 42 with VectorNd

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

the class MechSystemBase method getState.

// NEWX
public void getState(ComponentState pstate) {
    long t0 = System.nanoTime();
    if (!(pstate instanceof NumericState)) {
        throw new IllegalArgumentException("pstate not a NumericState");
    }
    NumericState state = (NumericState) pstate;
    state.clear();
    // get the required sizes
    updateAuxStateComponentList();
    updateDynamicComponentLists();
    updateForceComponentList();
    int numb = getNumBilateralImpulses();
    int numu = getNumUnilateralImpulses();
    state.zput(0x1234);
    state.zput(myNumActive + myNumParametric);
    state.zput(myAuxStateComponents.size());
    state.zput(myConstrainers.size());
    for (int i = 0; i < myNumActive + myNumParametric; i++) {
        DynamicComponent c = myDynamicComponents.get(i);
        int size = c.getVelStateSize() + c.getPosStateSize();
        int di = state.dsize();
        state.dsetSize(di + size);
        double[] dbuf = state.dbuffer();
        di = c.getPosState(dbuf, di);
        di = c.getVelState(dbuf, di);
    }
    getAuxState(state);
    state.dput(myLastBilateralH);
    state.dput(myLastUnilateralH);
    int di = state.dsize();
    state.dsetSize(di + numb + numu);
    // create special vector to access the state ...
    VectorNd dvec = new VectorNd();
    dvec.setBuffer(state.dsize(), state.dbuffer());
    for (int i = 0; i < myConstrainers.size(); i++) {
        Constrainer c = myConstrainers.get(i);
        di = c.getBilateralImpulses(dvec, di);
        di = c.getUnilateralImpulses(dvec, di);
    }
    long t1 = System.nanoTime();
// System.out.println ("getState=" + (t1-t0)*1e-6 + "msec");
}

Example 43 with VectorNd

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

the class MechSystemBase method setState.

// NEWX
public void setState(ComponentState pstate) {
    if (!(pstate instanceof NumericState)) {
        throw new IllegalArgumentException("pstate not a NumericState");
    }
    NumericState state = (NumericState) pstate;
    state.resetOffsets();
    updateAuxStateComponentList();
    updateDynamicComponentLists();
    updateForceComponentList();
    int chk = state.zget();
    if (chk != 0x1234) {
        System.out.println("zoffset=" + state.zoffset() + " size=" + state.dsize() + " " + state.zsize());
        throw new IllegalArgumentException("state checksum is " + chk + ", expecting " + 0x1234);
    }
    int numDynComps = state.zget();
    int numAuxStateComps = state.zget();
    int numConstrainers = state.zget();
    if (numDynComps != myNumActive + myNumParametric) {
        throw new IllegalArgumentException("state contains " + numDynComps + " active & parametric components, " + "expecting " + (myNumActive + myNumParametric));
    }
    if (numAuxStateComps != myAuxStateComponents.size()) {
        throw new IllegalArgumentException("number of AuxState components is " + numAuxStateComps + ", expecting " + myAuxStateComponents.size());
    }
    if (numConstrainers != -1 && numConstrainers != myConstrainers.size()) {
        throw new IllegalArgumentException("number of constrainers is " + numConstrainers + ", expecting " + myConstrainers.size());
    }
    int di = 0;
    double[] dbuf = state.dbuffer();
    for (int i = 0; i < myNumActive + myNumParametric; i++) {
        DynamicComponent c = myDynamicComponents.get(i);
        int size = c.getPosStateSize() + c.getVelStateSize();
        // XXX should check size here
        di = c.setPosState(dbuf, di);
        di = c.setVelState(dbuf, di);
    }
    updatePosState();
    updateVelState();
    state.dskip(di);
    // setting aux state must be done here because it may change the number
    // of bilateral and unilateral impulses expected by the constrainers
    setAuxState(state);
    // myLastBilateralH and myLastUnilateralH were not stored.
    if (state.doffset() == state.dsize() && numConstrainers == 0) {
        numConstrainers = -1;
    }
    // numConstrainers == -1 indicates initial state
    if (numConstrainers == -1) {
        myLastBilateralH = 1;
        myLastUnilateralH = 1;
        for (int i = 0; i < myConstrainers.size(); i++) {
            myConstrainers.get(i).zeroImpulses();
        }
    } else {
        myLastBilateralH = state.dget();
        myLastUnilateralH = state.dget();
        // create special vector to access the state ...
        VectorNd dvec = new VectorNd();
        dvec.setBuffer(state.dsize(), state.dbuffer());
        di = state.doffset();
        for (int i = 0; i < myConstrainers.size(); i++) {
            Constrainer c = myConstrainers.get(i);
            di = c.setBilateralImpulses(dvec, myLastBilateralH, di);
            di = c.setUnilateralImpulses(dvec, myLastUnilateralH, di);
        }
        state.dsetOffset(di);
    }
}

Example 44 with VectorNd

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

the class MFreeElement3d method getNaturalCoordinates.

/**
 * Given point p, get its natural coordinates with respect to this element.
 * Returns true if the algorithm converges, false if a maximum number of
 * iterations is reached. Uses a modified Newton's method to solve the
 * equations. The <code>coords</code> argument that returned the coordinates is
 * used, on input, to supply an initial guess of the coordinates.
 * Zero is generally a safe guess.
 *
 * @param coords
 * Outputs the natural coordinates, and supplies (on input) an initial
 * guess as to their position.
 * @param pnt
 * A given point (in world coords)
 * @param maxIters
 * Maximum number of Newton iterations
 * @param N
 * Resulting shape functionvalues
 * @return the number of iterations required for convergence, or
 * -1 if the calculation did not converge.
 */
public int getNaturalCoordinates(Point3d coords, Point3d pnt, int maxIters, VectorNd N) {
    Vector3d res = new Point3d();
    int i;
    double tol = ((MFreeNode3d) myNodes[0]).getInfluenceRadius() * 1e-12;
    if (tol <= 0) {
        tol = 1e-12;
    }
    if (N == null) {
        N = new VectorNd(numNodes());
    } else {
        N.setSize(numNodes());
    }
    // if (!coordsAreInside(coords)) {
    // return -1;
    // }
    myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
    computeNaturalCoordsResidual(res, coords, pnt, N);
    double prn = res.norm();
    // System.out.println ("res=" + prn);
    if (prn < tol) {
        // already have the right answer
        return 0;
    }
    LUDecomposition LUD = new LUDecomposition();
    Vector3d prevCoords = new Vector3d();
    Vector3d dNds = new Vector3d();
    Matrix3d dxds = new Matrix3d();
    Vector3d del = new Point3d();
    /*
       * solve using Newton's method.
       */
    for (i = 0; i < maxIters; i++) {
        // compute the Jacobian dx/ds for the current guess
        dxds.setZero();
        for (int k = 0; k < numNodes(); k++) {
            myShapeFunction.evalDerivative(k, dNds);
            dxds.addOuterProduct(myNodes[k].getPosition(), dNds);
        }
        LUD.factor(dxds);
        double cond = LUD.conditionEstimate(dxds);
        if (cond > 1e10)
            System.err.println("Warning: condition number for solving natural coordinates is " + cond);
        // solve Jacobian to obtain an update for the coords
        LUD.solve(del, res);
        prevCoords.set(coords);
        coords.sub(del);
        // if (!coordsAreInside(coords)) {
        // return -1;
        // }
        myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
        computeNaturalCoordsResidual(res, coords, pnt, N);
        double rn = res.norm();
        // If the residual norm is within tolerance, we have converged.
        if (rn < tol) {
            // System.out.println ("2 res=" + rn);
            return i + 1;
        }
        if (rn > prn) {
            // it may be that "coords + del" is a worse solution.  Let's make
            // sure we go the correct way binary search suitable alpha in [0 1]
            double eps = 1e-12;
            // and keep cutting the step size in half until the residual starts
            // dropping again
            double alpha = 0.5;
            while (alpha > eps && rn > prn) {
                coords.scaledAdd(-alpha, del, prevCoords);
                if (!coordsAreInside(coords)) {
                    return -1;
                }
                myShapeFunction.update(coords, (MFreeNode3d[]) myNodes);
                computeNaturalCoordsResidual(res, coords, pnt, N);
                rn = res.norm();
                alpha *= 0.5;
            // System.out.println ("  alpha=" + alpha + " rn=" + rn);
            }
            // System.out.println (" alpha=" + alpha + " rn=" + rn + " prn=" + prn);
            if (alpha < eps) {
                // failed
                return -1;
            }
        }
        prn = rn;
    }
    // failed
    return -1;
}

Example 45 with VectorNd

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

the class MFreeElement3d method getNodalExtrapolationMatrix.

// /**
// * Returns the number of pressure variables associated with this element.
// * All of the linear elements have one pressure variable, whereas some of
// * the quadratic elements have more.
// *
// * @return number of pressure variables
// *
// */
// public int numPressureVals() {
// // higher order elements should override this if necessary
// return 1;
// }
// 
// /**
// * Returns the pressure weight matrix for this element. The pressure
// * weight matrix is given by the inverse of the integral of
// * H^T H, where H is the row vector formed from the pressure
// * shape functions.
// *
// * <p>By default, this method returns a pressure weight matrix for the case
// * where there is only one pressure value. Such matrices always have a
// * single value of 1. Elements with a larger number of pressure values
// * should override this method to return a pressure weight matrix
// * appropriate for that element.
// */
// public Matrix getPressureWeightMatrix () {
// if (myPressureWeightMatrix == null) {
// myPressureWeightMatrix = new Matrix1x1();
// myPressureWeightMatrix.m00 = 1;
// }
// return myPressureWeightMatrix;
// }
// 
public double[] getNodalExtrapolationMatrix() {
    if (myNodalExtrapolationMatrix == null) {
        // build
        IntegrationData3d[] idata = getIntegrationData();
        IntegrationPoint3d[] ipnts = getIntegrationPoints();
        myNodalExtrapolationMatrix = new double[idata.length * myNodes.length];
        for (int k = 0; k < idata.length; ++k) {
            VectorNd N = ipnts[k].getShapeWeights();
            for (int j = 0; j < myNodes.length; ++j) {
                myNodalExtrapolationMatrix[j * ipnts.length + k] = N.get(j);
            }
        }
    }
    return myNodalExtrapolationMatrix;
}