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

Example 66 with RigidTransform3d

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

the class FixedMeshBody method setOrientation.

public void setOrientation(AxisAngle axisAng) {
    RigidTransform3d X = new RigidTransform3d(myState.XFrameToWorld);
    X.R.setAxisAngle(axisAng);
    setPose(X);
}

Example 67 with RigidTransform3d

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

the class Frame method copy.

public Frame copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    Frame comp = (Frame) super.copy(flags, copyMap);
    comp.myState = new FrameState();
    comp.myForce = new Wrench();
    comp.myExternalForce = new Wrench();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myAxisLength = myAxisLength;
    comp.mySolveBlock = null;
    comp.mySolveBlockNum = -1;
    comp.mySolveBlockValidP = false;
    comp.setPose(getPose());
    return comp;
}

Example 68 with RigidTransform3d

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

the class Frame method addPosImpulse.

public void addPosImpulse(double[] xbuf, int xidx, double h, double[] vbuf, int vidx) {
    Twist vel = new Twist();
    vel.v.x = vbuf[vidx++];
    vel.v.y = vbuf[vidx++];
    vel.v.z = vbuf[vidx++];
    vel.w.x = vbuf[vidx++];
    vel.w.y = vbuf[vidx++];
    vel.w.z = vbuf[vidx++];
    // XXX streamline this. This is only in the form it is to preserve exact
    // numeric compatibility with older code.
    RigidTransform3d X = new RigidTransform3d();
    X.p.set(xbuf[xidx], xbuf[xidx + 1], xbuf[xidx + 2]);
    Quaternion rot = new Quaternion(xbuf[xidx + 3], xbuf[xidx + 4], xbuf[xidx + 5], xbuf[xidx + 6]);
    rot.normalize();
    X.R.set(rot);
    if (dynamicVelInWorldCoords) {
        vel.extrapolateTransformWorld(X, h);
    } else {
        vel.extrapolateTransform(X, h);
    }
    rot.set(X.R);
    xbuf[xidx++] = X.p.x;
    xbuf[xidx++] = X.p.y;
    xbuf[xidx++] = X.p.z;
    xbuf[xidx++] = rot.s;
    xbuf[xidx++] = rot.u.x;
    xbuf[xidx++] = rot.u.y;
    xbuf[xidx++] = rot.u.z;
}

Example 69 with RigidTransform3d

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

the class MFreeFactory method generatePointLocations.

public static Point3d[] generatePointLocations(PolygonalMesh mesh, int[] res, int nPoints) {
    Vector3d centroid = new Vector3d();
    mesh.computeCentroid(centroid);
    RigidTransform3d trans = new RigidTransform3d();
    trans.setTranslation(centroid);
    OBB obb = PointDistributor.getTightOBB(mesh, trans);
    Vector3d widths = new Vector3d();
    obb.getWidths(widths);
    obb.getTransform(trans);
    int nx = res[0];
    int ny = res[1];
    int nz = res[2];
    double dx, dy, dz;
    double xOffset, yOffset, zOffset;
    if (nx == 1) {
        widths.x = 0;
        dx = 0;
    } else {
        dx = widths.x / (nx - 1);
    }
    if (ny == 1) {
        widths.y = 0;
        dy = 0;
    } else {
        dy = widths.y / (ny - 1);
    }
    if (nz == 1) {
        widths.z = 0;
        dz = 0;
    } else {
        dz = widths.z / (nz - 1);
    }
    xOffset = -widths.x / 2;
    yOffset = -widths.y / 2;
    zOffset = -widths.z / 2;
    // generate a grid of points that fall inside domain
    Point3d[][][] pnts = new Point3d[nx][ny][nz];
    // BVFeatureQuery query = new BVFeatureQuery();
    Vector3i sdres = new Vector3i(2 * nx, 2 * ny, 2 * nz);
    sdres.x = Math.min(sdres.x, 30);
    sdres.y = Math.min(sdres.y, 30);
    sdres.z = Math.min(sdres.z, 30);
    Logger.getSystemLogger().debug("Creating signed distance grid");
    List<Face> faces = mesh.getFaces();
    DistanceGrid sdgrid = new DistanceGrid(faces, 0.1, sdres, true);
    Logger.getSystemLogger().debug("done");
    double tol = 1e-15;
    Logger.getSystemLogger().debug("Generating " + nx + "x" + ny + "x" + nz + " points");
    double x, y, z;
    for (int i = 0; i < nx; i++) {
        x = xOffset + i * dx;
        for (int j = 0; j < ny; j++) {
            y = yOffset + j * dy;
            for (int k = 0; k < nz; k++) {
                z = zOffset + k * dz;
                Point3d pnt = new Point3d(x, y, z);
                pnt.transform(trans);
                double d = sdgrid.getLocalDistanceAndNormal(null, null, pnt);
                if (d < tol) {
                    pnts[i][j][k] = pnt;
                } else {
                    pnts[i][j][k] = null;
                }
            // InsideQuery rayTest = query.isInsideMesh(mesh, pnt, tol);
            // if (rayTest == InsideQuery.INSIDE) {
            // pnts[i][j][k] = pnt;
            // } else if (rayTest == InsideQuery.OUTSIDE) {
            // pnts[i][j][k] = null;
            // } else {
            // System.out.println("unsure");
            // }
            }
        }
    }
    Logger.getSystemLogger().debug("done.");
    Logger.getSystemLogger().debug("Farthest point sampling...");
    Point3dGridUtility pgu = new Point3dGridUtility(pnts);
    FastRadialMarcher marcher = new FastRadialMarcher(nx * ny * nz, pgu);
    marcher.initializeArrays();
    double[] dists = marcher.getDistance();
    // mark null points as having distance of -1 (never to be selected)
    for (int i = 0; i < nx; i++) {
        for (int j = 0; j < ny; j++) {
            for (int k = 0; k < nz; k++) {
                if (pnts[i][j][k] == null) {
                    dists[k + j * nz + i * ny * nz] = -1;
                }
            }
        }
    }
    // initialize heap to include all indices
    marcher.getDistanceHeap().setAll();
    // farthest-point sampling
    Point3d[] out = new Point3d[nPoints];
    int i, j, k;
    for (int idx = 0; idx < nPoints; idx++) {
        int farthest = 0;
        IndexedBinaryHeap dheap = marcher.getDistanceHeap();
        farthest = dheap.peek();
        // get next furthest
        int nextSample = farthest;
        // idx = i*ny*nz+j*nz+k
        k = nextSample % nz;
        j = ((nextSample - k) / nz) % ny;
        i = (nextSample - k - j * nz) / (ny * nz);
        out[idx] = pnts[i][j][k];
        // update distances to first point
        marcher.march(nextSample);
    }
    Logger.getSystemLogger().debug("done.");
    return out;
}

Example 70 with RigidTransform3d

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

the class RevoluteJoint method setTheta.

public void setTheta(double theta) {
    theta = myThetaRange.makeValid(theta);
    RigidTransform3d TGD = new RigidTransform3d();
    ((RevoluteCoupling) myCoupling).setTheta(TGD, Math.toRadians(theta));
    if (getParent() != null) {
        // if we are connected to the hierarchy, adjust the poses of the
        // attached bodies appropriately.
        adjustPoses(TGD);
    }
}