Examples with ConstraintInfo artisynth.core.mechmodels.MechSystem.ConstraintInfo used on opensource projects

Example 1 with ConstraintInfo

use of artisynth.core.mechmodels.MechSystem.ConstraintInfo in project artisynth_core by artisynth.

the class MechSystemSolver method computePosCorrections.

protected void computePosCorrections(VectorNd pos, VectorNd vel, double t) {
    boolean correctionNeeded = false;
    // assumes that updateMassMatrix() has been called
    int velSize = myActiveVelSize;
    if (velSize == 0) {
        return;
    }
    if (myConSolver == null) {
        myConSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    updateUnilateralConstraints();
    // myVel.setSize (velSize);
    if (myGsize > 0 || myNsize > 0) {
        boolean allConstraintsCompliant = true;
        mySys.getBilateralInfo(myGInfo);
        double[] gbuf = myBg.getBuffer();
        for (int i = 0; i < myGsize; i++) {
            ConstraintInfo gi = myGInfo[i];
            if (!myComplianceSupported || gi.compliance == 0) {
                gbuf[i] = -myGInfo[i].dist;
                allConstraintsCompliant = false;
            } else {
                gbuf[i] = 0;
            }
        }
        myRg.setZero();
        // mySys.getUnilateralOffsets (myRn, myBn, 0, MechSystem.POSITION_MODE);
        mySys.getUnilateralInfo(myNInfo);
        double[] nbuf = myBn.getBuffer();
        for (int i = 0; i < myNsize; i++) {
            ConstraintInfo ni = myNInfo[i];
            if (!myComplianceSupported || ni.compliance == 0) {
                nbuf[i] = -myNInfo[i].dist;
                allConstraintsCompliant = false;
            } else {
                nbuf[i] = 0;
            }
        }
        // only need to do the correction if some constraints are non-compliant
        if (!allConstraintsCompliant) {
            correctionNeeded = true;
            myRg.setZero();
            myRn.setZero();
            // System.out.println ("bn=" + myBn);
            myBf.setSize(velSize);
            myBf.setZero();
            if (myStabilization == PosStabilization.GlobalStiffness && integratorIsImplicit(myIntegrator)) {
                computeStiffnessPosCorrection(vel, velSize);
            } else {
                computeMassPosCorrection(vel, velSize);
            }
        }
    }
    if (correctionNeeded) {
        mySys.addActivePosImpulse(pos, 1, vel);
    }
}

Example 2 with ConstraintInfo

use of artisynth.core.mechmodels.MechSystem.ConstraintInfo in project artisynth_core by artisynth.

the class MechSystemSolver method setBilateralOffsets.

private void setBilateralOffsets(double h, double dotscale) {
    if (myGsize > 0) {
        mySys.getBilateralInfo(myGInfo);
        double[] gdot = myGdot.getBuffer();
        double[] Rbuf = myRg.getBuffer();
        double[] gbuf = myBg.getBuffer();
        for (int i = 0; i < myGsize; i++) {
            ConstraintInfo gi = myGInfo[i];
            if (gi.compliance > 0) {
                if (gi.force != 0) {
                    double alpha = 1 / (0.5 * h / gi.compliance + gi.damping);
                    Rbuf[i] = alpha / h;
                    gbuf[i] -= alpha * gi.force;
                } else {
                    double s = 1 / (0.5 * h + gi.damping * gi.compliance);
                    Rbuf[i] = s * gi.compliance / h;
                    gbuf[i] -= s * gi.dist;
                }
            }
            gbuf[i] -= dotscale * gdot[i];
        // System.out.println ("gbuf=" + gbuf[i]);
        }
    }
}

Example 3 with ConstraintInfo

use of artisynth.core.mechmodels.MechSystem.ConstraintInfo in project artisynth_core by artisynth.

the class CollisionHandler method getUnilateralInfo.

@Override
public int getUnilateralInfo(ConstraintInfo[] ninfo, int idx) {
    double[] fres = new double[] { 0, myCompliance, myDamping };
    ContactForceBehavior forceBehavior = getForceBehavior();
    for (int i = 0; i < myUnilaterals.size(); i++) {
        ContactConstraint c = myUnilaterals.get(i);
        c.setSolveIndex(idx);
        ConstraintInfo ni = ninfo[idx++];
        if (c.getDistance() < -myBehavior.myPenetrationTol) {
            ni.dist = (c.getDistance() + myBehavior.myPenetrationTol);
        } else {
            ni.dist = 0;
        }
        if (forceBehavior != null) {
            forceBehavior.computeResponse(fres, c.myDistance, c.myCpnt0, c.myCpnt1, c.myNormal, c.myRegion);
        }
        ni.force = fres[0];
        ni.compliance = fres[1];
        ni.damping = fres[2];
    }
    return idx;
}

Example 4 with ConstraintInfo

use of artisynth.core.mechmodels.MechSystem.ConstraintInfo in project artisynth_core by artisynth.

the class CollisionHandler method getBilateralInfo.

@Override
public int getBilateralInfo(ConstraintInfo[] ginfo, int idx) {
    double[] fres = new double[] { 0, myCompliance, myDamping };
    ContactForceBehavior forceBehavior = getForceBehavior();
    for (ContactConstraint c : myBilaterals0.values()) {
        c.setSolveIndex(idx);
        ConstraintInfo gi = ginfo[idx++];
        if (c.getDistance() < -myBehavior.myPenetrationTol) {
            gi.dist = (c.getDistance() + myBehavior.myPenetrationTol);
        } else {
            gi.dist = 0;
        }
        if (forceBehavior != null) {
            forceBehavior.computeResponse(fres, c.myDistance, c.myCpnt0, c.myCpnt1, c.myNormal, c.myRegion);
        }
        gi.force = fres[0];
        gi.compliance = fres[1];
        gi.damping = fres[2];
    }
    for (ContactConstraint c : myBilaterals1.values()) {
        c.setSolveIndex(idx);
        ConstraintInfo gi = ginfo[idx++];
        if (c.getDistance() < -myBehavior.myPenetrationTol) {
            gi.dist = (c.getDistance() + myBehavior.myPenetrationTol);
        } else {
            gi.dist = 0;
        }
        if (forceBehavior != null) {
            forceBehavior.computeResponse(fres, c.myDistance, c.myCpnt0, c.myCpnt1, c.myNormal, c.myRegion);
        }
        gi.force = fres[0];
        gi.compliance = fres[1];
        gi.damping = fres[2];
    }
    return idx;
}

Example 5 with ConstraintInfo

use of artisynth.core.mechmodels.MechSystem.ConstraintInfo in project artisynth_core by artisynth.

the class LinearPointConstraint method getBilateralInfo.

@Override
public int getBilateralInfo(ConstraintInfo[] ginfo, int idx) {
    sumPos.setZero();
    int nValid = 0;
    for (int i = 0; i < myPoints.length; i++) {
        Point pnt = myPoints[i];
        if (pnt.getSolveIndex() > -1) {
            nValid++;
        }
        sumPos.scaledAdd(myWgts[i], pnt.getPosition());
    }
    if (nValid == 0) {
        return idx;
    }
    // x
    ConstraintInfo gi = ginfo[idx++];
    gi.dist = sumPos.x;
    gi.compliance = 0;
    gi.damping = 0;
    gi.force = 0;
    // y
    gi = ginfo[idx++];
    gi.dist = sumPos.y;
    gi.compliance = 0;
    gi.damping = 0;
    gi.force = 0;
    // z
    gi = ginfo[idx++];
    gi.dist = sumPos.z;
    gi.compliance = 0;
    gi.damping = 0;
    gi.force = 0;
    return idx;
}