Examples with SpatialInertia maspack.spatialmotion.SpatialInertia used on opensource projects

Example 11 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia in project artisynth_core by artisynth.

the class RigidBody method scanItem.

protected boolean scanItem(ReaderTokenizer rtok, Deque<ScanToken> tokens) throws IOException {
    rtok.nextToken();
    if (scanAttributeName(rtok, "mesh")) {
        myMeshInfo.scan(rtok);
        setMeshFromInfo();
        return true;
    } else if (scanAttributeName(rtok, "pose")) {
        RigidTransform3d X = new RigidTransform3d();
        X.scan(rtok);
        myState.setPose(X);
        return true;
    } else if (scanAttributeName(rtok, "position")) {
        Point3d pos = new Point3d();
        pos.scan(rtok);
        setPosition(pos);
        return true;
    } else if (scanAttributeName(rtok, "rotation")) {
        Quaternion q = new Quaternion();
        q.scan(rtok);
        setRotation(q);
        return true;
    } else if (scanAttributeName(rtok, "vel")) {
        rtok.scanToken('[');
        myState.vel.scan(rtok);
        rtok.scanToken(']');
        return true;
    } else if (scanAttributeName(rtok, "inertia")) {
        SpatialInertia M = new SpatialInertia();
        M.scan(rtok);
        setInertia(M);
        return true;
    } else if (scanAttributeName(rtok, "mass")) {
        double mass = rtok.scanNumber();
        setMass(mass);
        return true;
    } else if (scanAttributeName(rtok, "density")) {
        double density = rtok.scanNumber();
        setDensity(density);
        return true;
    }
    rtok.pushBack();
    return super.scanItem(rtok, tokens);
}

Example 12 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia in project artisynth_core by artisynth.

the class RigidBody method copy.

@Override
public RigidBody copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    RigidBody comp = (RigidBody) super.copy(flags, copyMap);
    // comp.myAttachments = new ArrayList<PointFrameAttachment>();
    // comp.myComponents = new ArrayList<ModelComponent>();
    // comp.indicesValidP = false;
    comp.setDynamic(true);
    comp.mySpatialInertia = new SpatialInertia(mySpatialInertia);
    // comp.myEffectiveInertia = new SpatialInertia (mySpatialInertia);
    PolygonalMesh mesh = getMesh();
    comp.myMeshInfo = new MeshInfo();
    if (mesh != null) {
        PolygonalMesh meshCopy = mesh.copy();
        comp.setMesh(meshCopy, getMeshFileName(), getMeshFileTransform());
        comp.myMeshInfo.myFlippedP = myMeshInfo.myFlippedP;
    } else {
        comp.setMesh(null, null, null);
    }
    // comp.myMarkers =
    // new PointList<FrameMarker> (
    // FrameMarker.class, "markers", "k");
    // comp.add (comp.myMarkers);
    comp.myBodyForce = new Wrench();
    comp.myCoriolisForce = new Wrench();
    comp.myBodyVel = new Twist();
    comp.myBodyAcc = new Twist();
    comp.myQvel = new Quaternion();
    comp.myTmpPos = new Point3d();
    comp.myConnectors = null;
    return comp;
}

Example 13 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia in project artisynth_core by artisynth.

the class RigidBody method applyGravity.

// public void updatePose() {
// super.updatePose();
// updatePosState();
// }
public void applyGravity(Vector3d gacc) {
    // apply a force of -mass gacc at the bodies's center of mass
    Point3d com = new Point3d();
    Vector3d fgrav = new Vector3d();
    // SpatialInertia inertia = MechModel.getEffectiveInertia(this);
    SpatialInertia inertia = mySpatialInertia;
    inertia.getCenterOfMass(com);
    com.transform(myState.XFrameToWorld.R);
    fgrav.scale(inertia.getMass(), gacc);
    myForce.f.add(fgrav, myForce.f);
    myForce.m.crossAdd(com, fgrav, myForce.m);
}

Example 14 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia in project artisynth_core by artisynth.

the class RigidBody method setInertia.

/**
 * Explicitly sets the mass and rotational inertia of this body. Also sets
 * the uniform density (as returned by {@link #getDensity getDensity}) to be
 * undefined).
 */
public void setInertia(double m, SymmetricMatrix3d J) {
    SpatialInertia M = new SpatialInertia();
    M.set(m, J);
    doSetInertia(M);
}

Example 15 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia in project artisynth_core by artisynth.

the class RigidCompositeBody method setInertia.

/**
 * Explicitly sets the mass, rotational inertia, and center of mass of this
 * body. Also sets the uniform density (as returned by {@link #getDensity
 * getDensity}) to be undefined).
 */
public void setInertia(double m, SymmetricMatrix3d J, Point3d com) {
    SpatialInertia M = new SpatialInertia();
    M.set(m, J, com);
    doSetInertia(M);
}