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

Example 1 with AffineTransform3d

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

the class CPDTest method main.

public static void main(String[] args) {
    AffineTransform3d trans = new AffineTransform3d();
    RotationMatrix3d R = new RotationMatrix3d(0.7605, -0.6307, 0.1541, 0.6485, 0.7263, -0.2279, 0.0318, 0.2733, 0.9614);
    double s = 2.7;
    trans.setA(R, new Vector3d(s, s, s), new Vector3d(0, 0, 0));
    trans.setTranslation(new Vector3d(1, 2, 3));
    Point3d[] X = get3DFish();
    int N = X.length;
    int M = N - 20;
    Point3d[] Y = new Point3d[M];
    Point3d[] out = new Point3d[M];
    for (int i = 0; i < N; i++) {
        if (i < M) {
            Y[i] = new Point3d(X[i]);
            out[i] = new Point3d();
        }
        X[i].transform(trans);
    }
    double w = 0.01;
    double lambda = 0.1;
    double beta2 = 3.5;
    CPD.verbose = true;
    ScaledRigidTransform3d rigidT = CPD.rigid(X, Y, w, 1e-10, 100, true, out);
    AffineTransform3d affT = CPD.affine(X, Y, w, 1e-10, 100, out);
    CPD.coherent(X, Y, lambda, beta2, w, 1e-10, 100, out);
    System.out.println("Original: \n" + trans.toString());
    System.out.println("Rigid Computed: \n" + rigidT.toString());
    System.out.println("Affine Computed: \n" + affT.toString());
    for (int i = 0; i < M; i++) {
        System.out.println(X[i].toString() + "\n" + out[i].toString() + "\n");
    }
}

Example 2 with AffineTransform3d

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

the class MeshICPAligner method doAlignment.

public void doAlignment(File base, File in, OutputStream out) {
    PolygonalMesh mesh1, mesh2, mesh3;
    try {
        mesh1 = new PolygonalMesh(base);
        mesh2 = new PolygonalMesh(in);
        mesh3 = new PolygonalMesh();
    } catch (IOException e) {
        e.printStackTrace();
        return;
    }
    // do alignment
    AffineTransform3d trans = MeshICP.align(mesh1, mesh2, AlignmentType.RIGID_WITH_SCALING, 1e-12, 100, mesh3);
    try {
        // mesh3.write(new PrintWriter(out), "%g");
        trans.write(new PrintWriter(out), "%g", null);
        out.flush();
    } catch (IOException e) {
        e.printStackTrace();
    }
    if (out != System.out) {
        try {
            out.close();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }
}

Example 3 with AffineTransform3d

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

the class MayaAsciiReader method getPolylineMesh.

public PolylineMesh getPolylineMesh(Node<MayaNode> root, UnitInfo units, String regex) {
    if (units == null) {
        units = defaultUnits;
    }
    PolylineMesh mesh = new PolylineMesh();
    AffineTransform3d trans = new AffineTransform3d();
    recursiveBuildParentTransform(root, trans, units);
    Pattern pregex = null;
    if (regex != null) {
        pregex = Pattern.compile(regex);
    }
    recursiveAddPolylines(root, mesh, trans, units, pregex);
    return mesh;
}

Example 4 with AffineTransform3d

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

the class MayaAsciiReader method recursiveAddPolygonalMeshes.

private void recursiveAddPolygonalMeshes(Node<MayaNode> root, PolygonalMesh mesh, AffineTransform3d trans, UnitInfo units) {
    // make copy so can traverse
    trans = new AffineTransform3d(trans);
    // children independently
    MayaNode data = root.getData();
    if (data instanceof MayaTransform) {
        MayaTransform dtrans = (MayaTransform) data;
        AffineTransform3d tu = new AffineTransform3d();
        dtrans.getTransform(tu);
        // convert units
        tu.p.scale(dtrans.units.length.getSI() / units.length.getSI());
        // only multiply if inherited
        if (dtrans.inheritsTransform()) {
            trans.mul(tu);
        } else {
            trans.set(tu);
        }
    } else if (data instanceof MayaMesh) {
        MayaMesh mm = (MayaMesh) data;
        PolygonalMesh mmesh = mm.getMesh();
        if (mmesh != null) {
            // transform mesh
            HashMap<Vertex3d, Vertex3d> vtxMap = new HashMap<Vertex3d, Vertex3d>();
            for (Vertex3d vtx : mmesh.getVertices()) {
                Vertex3d nvtx = new Vertex3d(vtx.pnt);
                // XXX prevent transform
                nvtx.pnt.scale(mm.units.length.getSI() / units.length.getSI());
                nvtx.pnt.transform(trans);
                vtxMap.put(vtx, nvtx);
                mesh.addVertex(nvtx);
            }
            for (Face face : mmesh.getFaces()) {
                Vertex3d[] oface = face.getVertices();
                Vertex3d[] nface = new Vertex3d[oface.length];
                for (int i = 0; i < oface.length; i++) {
                    nface[i] = vtxMap.get(oface[i]);
                }
                mesh.addFace(nface);
            }
        }
    }
    for (Node<MayaNode> child : root.getChildren()) {
        recursiveAddPolygonalMeshes(child, mesh, trans, units);
    }
}

Example 5 with AffineTransform3d

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

the class DicomPlaneTextureContent method updateBackingBuffer.

/**
 * Uploads dicom pixels to backing buffer
 * @param plane plane index
 */
protected void updateBackingBuffer() {
    int psize = getPixelSize();
    int scanline = psize * res.x;
    // voxels into spatial
    AffineTransform3d vtrans = image.getVoxelTransform();
    // spatial into voxels
    vtrans.invert();
    // point for computing location
    Point3d pnt = new Point3d();
    Point3d vpnt = new Point3d();
    double dx = widths.x / rect.width();
    double dy = widths.y / rect.height();
    synchronized (textureImage) {
        textureImage.limit(textureImage.capacity());
        for (int j = 0; j < rect.height(); ++j) {
            textureImage.position((rect.y() + j) * scanline + psize * rect.x());
            for (int i = 0; i < rect.width(); ++i) {
                pnt.x = (-widths.x + (2 * i + 1) * dx) / 2.0;
                pnt.y = (-widths.y + (2 * j + 1) * dy) / 2.0;
                pnt.z = 0;
                // world coordinate
                vpnt.transform(location, pnt);
                // voxel coordinate
                vpnt.transform(vtrans);
                int vx = (int) Math.floor(vpnt.x);
                int vy = (int) Math.floor(vpnt.y);
                int vz = (int) Math.floor(vpnt.z);
                // interpolate around voxel
                int[] voxel = new int[3];
                switch(internalStorage) {
                    case UBYTE:
                    case BYTE:
                        {
                            double val = 0;
                            if (interpolate) {
                                // scale factors for interp
                                double cx = (vpnt.x - vx) / image.pixelSpacingCols;
                                double cy = (vpnt.y - vy) / image.pixelSpacingRows;
                                double cz = (vpnt.z - vz) / image.pixelSpacingSlice;
                                double[][] cc = { { 1 - cx, cx }, { 1 - cy, cy }, { 1 - cz, cz } };
                                int xmin = Math.max(vx, 0);
                                int xmax = Math.min(vx + 2, image.getNumCols() - 1);
                                int ymin = Math.max(vy, 0);
                                int ymax = Math.min(vy + 2, image.getNumRows() - 1);
                                int zmin = Math.max(vz, 0);
                                int zmax = Math.min(vz + 2, image.getNumSlices() - 1);
                                for (int xx = xmin; xx < xmax; ++xx) {
                                    for (int yy = ymin; yy < ymax; ++yy) {
                                        for (int zz = zmin; zz < zmax; ++zz) {
                                            image.getPixels(xx, yy, zz, 0, 0, 0, 1, 1, 1, PixelType.UBYTE, 0, 0, window, voxel, 0);
                                            // interpolate
                                            val += cc[0][xx - vx] * cc[1][yy - vy] * cc[2][zz - vz] * (voxel[0]);
                                        }
                                    }
                                }
                            } else {
                                int vvx = (int) Math.round(vpnt.x);
                                int vvy = (int) Math.round(vpnt.y);
                                int vvz = (int) Math.round(vpnt.z);
                                if (vvx >= 0 && vvx < image.getNumCols() && vvy >= 0 && vvy < image.getNumRows() && vvz >= 0 && vvz < image.getNumSlices()) {
                                    image.getPixels(vvx, vvy, vvz, 0, 0, 0, 1, 1, 1, PixelType.UBYTE, 0, 0, window, voxel, 0);
                                    val = voxel[0];
                                }
                            }
                            // round and convert to byte
                            textureImage.put((byte) Math.round(val));
                            break;
                        }
                    case UBYTE_RGB:
                        {
                            double rval = 0;
                            double gval = 0;
                            double bval = 0;
                            if (interpolate) {
                                // scale factors for interp
                                double cx = (vpnt.x - vx) / image.pixelSpacingCols;
                                double cy = (vpnt.y - vy) / image.pixelSpacingRows;
                                double cz = (vpnt.z - vz) / image.pixelSpacingSlice;
                                double[][] cc = { { 1 - cx, cx }, { 1 - cy, cy }, { 1 - cz, cz } };
                                int xmin = Math.max(vx, 0);
                                int xmax = Math.min(vx + 2, image.getNumCols() - 1);
                                int ymin = Math.max(vy, 0);
                                int ymax = Math.min(vy + 2, image.getNumRows() - 1);
                                int zmin = Math.max(vz, 0);
                                int zmax = Math.min(vz + 2, image.getNumSlices() - 1);
                                for (int xx = xmin; xx < xmax; ++xx) {
                                    for (int yy = ymin; yy < ymax; ++yy) {
                                        for (int zz = zmin; zz < zmax; ++zz) {
                                            image.getPixels(xx, yy, zz, 0, 0, 0, 1, 1, 1, PixelType.UBYTE_RGB, 0, 0, window, voxel, 0);
                                            // interpolate
                                            double ccc = cc[0][xx - vx] * cc[1][yy - vy] * cc[2][zz - vz];
                                            rval += ccc * (voxel[0]);
                                            gval += ccc * (voxel[1]);
                                            bval += ccc * (voxel[2]);
                                        }
                                    }
                                }
                            } else {
                                int vvx = (int) Math.round(vpnt.x);
                                int vvy = (int) Math.round(vpnt.y);
                                int vvz = (int) Math.round(vpnt.z);
                                if (vvx >= 0 && vvx < image.getNumCols() && vvy >= 0 && vvy < image.getNumRows() && vvz >= 0 && vvz < image.getNumSlices()) {
                                    image.getPixels(vvx, vvy, vvz, 0, 0, 0, 1, 1, 1, PixelType.UBYTE_RGB, 0, 0, window, voxel, 0);
                                    rval = voxel[0];
                                    gval = voxel[1];
                                    bval = voxel[2];
                                }
                            }
                            // round and convert to byte
                            textureImage.put((byte) Math.round(rval));
                            textureImage.put((byte) Math.round(gval));
                            textureImage.put((byte) Math.round(bval));
                            break;
                        }
                    case USHORT:
                    case SHORT:
                        {
                            double val = 0;
                            if (interpolate) {
                                // scale factors for interp, pixel space have widths of 1
                                double cx = (vpnt.x - vx);
                                double cy = (vpnt.y - vy);
                                double cz = (vpnt.z - vz);
                                double[][] cc = { { 1 - cx, cx }, { 1 - cy, cy }, { 1 - cz, cz } };
                                int xmin = Math.max(vx, 0);
                                int xmax = Math.min(vx + 2, image.getNumCols() - 1);
                                int ymin = Math.max(vy, 0);
                                int ymax = Math.min(vy + 2, image.getNumRows() - 1);
                                int zmin = Math.max(vz, 0);
                                int zmax = Math.min(vz + 2, image.getNumSlices() - 1);
                                for (int xx = xmin; xx < xmax; ++xx) {
                                    for (int yy = ymin; yy < ymax; ++yy) {
                                        for (int zz = zmin; zz < zmax; ++zz) {
                                            image.getPixels(xx, yy, zz, 0, 0, 0, 1, 1, 1, PixelType.USHORT, 0, 0, window, voxel, 0);
                                            // interpolate
                                            val += cc[0][xx - vx] * cc[1][yy - vy] * cc[2][zz - vz] * (voxel[0]);
                                        }
                                    }
                                }
                            } else {
                                int vvx = (int) Math.round(vpnt.x);
                                int vvy = (int) Math.round(vpnt.y);
                                int vvz = (int) Math.round(vpnt.z);
                                if (vvx >= 0 && vvx < image.getNumCols() && vvy >= 0 && vvy < image.getNumRows() && vvz >= 0 && vvz < image.getNumSlices()) {
                                    image.getPixels(vvx, vvy, vvz, 0, 0, 0, 1, 1, 1, PixelType.USHORT, 0, 0, window, voxel, 0);
                                    val = voxel[0];
                                }
                            }
                            // round and convert to short
                            textureImage.putShort((short) (Math.round(val)));
                            break;
                        }
                }
            }
        }
        valid = true;
    }
}