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

Example 6 with SpatialInertia

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

the class PolygonalMesh method createInertia.

/**
 * Creates a spatial inertia for the volume defined by this mesh, assuming a
 * constant density. It is assumed that the mesh is closed, although small
 * holes in the mesh should not affect the calculation that much.  Is is
 * also assumed that the faces are oriented counter-clockwise about their
 * outward-facing normal.
 *
 * @param density
 * density of the volume
 */
public SpatialInertia createInertia(double density) {
    SpatialInertia M = new SpatialInertia();
    computeInertia(M, density);
    return M;
}

Example 7 with SpatialInertia

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

the class PolygonalMeshTest method inertiaTest.

public void inertiaTest() {
    SpatialInertia M = new SpatialInertia();
    SpatialInertia Mcheck = new SpatialInertia();
    double density = 3;
    double tol = 1e-7;
    PolygonalMesh ellipsoid = MeshFactory.createSphere(2.0, 12);
    ellipsoid.scale(3, 2, 1);
    M = ellipsoid.createInertia(density);
    Mcheck = new SpatialInertia(537.41531629, 1980.71973353, 3943.79254142, 5103.98930052);
    checkInertia(M, Mcheck, tol);
    checkTranslatedInertia(ellipsoid, M, density, 1, 2, 3, tol);
    PolygonalMesh torus = MeshFactory.createTorus(2.0, 1.0, 24, 16);
    M = torus.createInertia(density);
    Mcheck = new SpatialInertia(114.10071614, 294.63934170, 294.63934170, 533.67589197);
    checkInertia(M, Mcheck, tol);
    checkTranslatedInertia(torus, M, density, 1, 2, 3, tol);
}

Example 8 with SpatialInertia

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

the class PolygonalMeshTest method checkTranslatedInertia.

private void checkTranslatedInertia(PolygonalMesh mesh, SpatialInertia M, double density, double cx, double cy, double cz, double tol) {
    PolygonalMesh tmesh = new PolygonalMesh(mesh);
    tmesh.transform(new RigidTransform3d(cx, cy, cz));
    SpatialInertia MT = tmesh.createInertia(density);
    SpatialInertia MTcheck = new SpatialInertia();
    Point3d com = new Point3d();
    M.getCenterOfMass(com);
    com.add(new Vector3d(cx, cy, cz));
    MTcheck.set(M.getMass(), M.getRotationalInertia(), com);
    checkInertia(MT, MTcheck, tol);
}

Example 9 with SpatialInertia

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

the class FemModel3d method getMassMatrixValues.

@Override
public void getMassMatrixValues(SparseBlockMatrix M, VectorNd f, double t) {
    int bk;
    for (int k = 0; k < myNodes.size(); k++) {
        FemNode3d n = myNodes.get(k);
        if ((bk = n.getSolveIndex()) != -1) {
            n.getEffectiveMass(M.getBlock(bk, bk), t);
            n.getEffectiveMassForces(f, t, M.getBlockRowOffset(bk));
        }
    }
    if (myFrameRelativeP) {
        // angular velocity in body coords
        Vector3d wbod = new Vector3d();
        RotationMatrix3d R = myFrame.getPose().R;
        double[] fbuf = f.getBuffer();
        wbod.inverseTransform(R, myFrame.getVelocity().w);
        Vector3d w = myFrame.getVelocity().w;
        // update inertia in Frame
        double mass = 0;
        Point3d com = new Point3d();
        SymmetricMatrix3d J = new SymmetricMatrix3d();
        // extra fictitious forces for spatial inertia due to nodal velocity
        Vector3d fv = new Vector3d();
        Vector3d fw = new Vector3d();
        Vector3d fn = new Vector3d();
        Vector3d tmp = new Vector3d();
        // local node position rotated to world
        Point3d c = new Point3d();
        // local node velocity rotated to world
        Vector3d v = new Vector3d();
        int bf = myFrame.getSolveIndex();
        for (int k = 0; k < myNodes.size(); k++) {
            FemNode3d n = myNodes.get(k);
            if ((bk = n.getSolveIndex()) != -1) {
                c.transform(R, n.getLocalPosition());
                v.transform(R, n.getLocalVelocity());
                double m = n.getEffectiveMass();
                // System.out.println ("m " + k + " " + m);
                com.scaledAdd(m, c);
                mass += m;
                SpatialInertia.addPointRotationalInertia(J, m, c);
                if (useFrameRelativeCouplingMasses) {
                    Matrix3x6Block blk = (Matrix3x6Block) M.getBlock(bk, bf);
                    Matrix6x3Block blkT = (Matrix6x3Block) M.getBlock(bf, bk);
                    setCouplingMass(blk, m, R, c);
                    blkT.transpose(blk);
                    // compute fictitious forces for nodes, and accumulate nodal
                    // velocity fictitious force terms for spatial inertia
                    // tmp = 2*m (w X v)
                    tmp.cross(w, v);
                    tmp.scale(2 * m);
                    // fv += 2*m (w X v)
                    fv.add(tmp);
                    // fw += 2*m (c X w X v)
                    fw.crossAdd(c, tmp, fw);
                    // fn = 2*m (w X v)
                    fn.set(tmp);
                    // tmp = m*w X c
                    tmp.cross(w, c);
                    tmp.scale(m);
                    // fn += m (w X w X c)
                    fn.crossAdd(w, tmp, fn);
                    fn.inverseTransform(R);
                    // set fictitious force terms for node
                    int idx = M.getBlockRowOffset(bk);
                    fbuf[idx++] = -fn.x;
                    fbuf[idx++] = -fn.y;
                    fbuf[idx++] = -fn.z;
                }
            }
        }
        com.scale(1 / mass);
        SpatialInertia.addPointRotationalInertia(J, -mass, com);
        SpatialInertia S = new SpatialInertia();
        S.set(mass, J, com);
        // Frame keeps inertia in local coords
        S.inverseTransform(R);
        myFrame.setInertia(S);
        myFrame.getMass(M.getBlock(bf, bf), t);
        myFrame.getEffectiveMassForces(f, t, M.getBlockRowOffset(bf));
        if (useFrameRelativeCouplingMasses) {
            int idx = M.getBlockRowOffset(bf);
            fbuf[idx++] -= fv.x;
            fbuf[idx++] -= fv.y;
            fbuf[idx++] -= fv.z;
            fbuf[idx++] -= fw.x;
            fbuf[idx++] -= fw.y;
            fbuf[idx++] -= fw.z;
        }
        if (frameMassFraction != 0) {
            scaleFrameNodeMasses(M, f, 1.0 - frameMassFraction);
        }
    }
}

Example 10 with SpatialInertia

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

the class RigidBody method getEffectiveMassForces.

public static int getEffectiveMassForces(VectorNd f, double t, FrameState state, SpatialInertia effectiveInertia, int idx) {
    Wrench coriolisForce = new Wrench();
    Twist bodyVel = new Twist();
    bodyVel.inverseTransform(state.XFrameToWorld.R, state.vel);
    SpatialInertia S = effectiveInertia;
    S.coriolisForce(coriolisForce, bodyVel);
    if (dynamicVelInWorldCoords) {
        coriolisForce.transform(state.XFrameToWorld.R);
    }
    double[] buf = f.getBuffer();
    buf[idx++] = -coriolisForce.f.x;
    buf[idx++] = -coriolisForce.f.y;
    buf[idx++] = -coriolisForce.f.z;
    buf[idx++] = -coriolisForce.m.x;
    buf[idx++] = -coriolisForce.m.y;
    buf[idx++] = -coriolisForce.m.z;
    return idx;
}