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

Example 41 with AffineTransform3d

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

the class MeshTransform method main.

// new StringHolder ("maspack.geometry.PolygonalMesh");
public static void main(String[] args) {
    ArgParser parser = new ArgParser("[options] <infileName>");
    // parser.addOption ("-inFile %s #input file name", inFileName);
    parser.addOption("-out %s #output file name", outFileName);
    parser.addOption("-xyz %fX3 #x,y,z translation values", xyz);
    parser.addOption("-scaleXyz %fX3 #x,y,z scale values", scaleXyz);
    parser.addOption("-laplacianSmooth %fX3 #iter count, lambda, mu", laplacianSmoothing);
    parser.addOption("-removeDisconnectedFaces %v #remove disconnected faces", removeDisconnectedFaces);
    parser.addOption("-removeDisconnectedVertices %v #remove disconnected vertices", removeDisconnectedVertices);
    parser.addOption("-mergeCloseVertices %f #(close vertex distance)/(mesh radius)", closeVertexTol);
    parser.addOption("-axisAngle %fX4 #x,y,z,deg axis-angle values", axisAngle);
    parser.addOption("-scale %f #scaling value", scaling);
    parser.addOption("-format %s #printf-syle format string for vertex output", formatStr);
    // parser.addOption (
    // "-zeroIndexedIn %v #input is zero indexed", zeroIndexedIn);
    // parser.addOption (
    // "-zeroIndexedOut %v #output should be zero indexed", zeroIndexedOut);
    parser.addOption("-class %s #use PolygonalMesh sub class", className);
    parser.addOption("-pointMesh %v #mesh is a point cloud", pointMesh);
    int idx = 0;
    while (idx < args.length) {
        try {
            idx = parser.matchArg(args, idx);
            if (parser.getUnmatchedArgument() != null) {
                String fileName = parser.getUnmatchedArgument();
                if (inFileName.value == null) {
                    inFileName.value = fileName;
                } else {
                    System.out.println("Ignoring extra input " + fileName);
                }
            }
        } catch (Exception e) {
            // malformed or erroneous argument
            parser.printErrorAndExit(e.getMessage());
        }
    }
    MeshBase mesh = null;
    if (inFileName.value == null) {
        parser.printErrorAndExit("input file name missing");
    }
    if (outFileName.value == null) {
        outFileName.value = inFileName.value;
    }
    File outFile = new File(outFileName.value);
    if (outFile.exists()) {
        System.out.print("File " + outFileName.value + " exists. Overwrite? (y/n) ");
        System.out.flush();
        BufferedReader reader = new BufferedReader(new InputStreamReader(System.in));
        String input = null;
        try {
            input = reader.readLine();
        } catch (Exception e) {
        // ignore
        }
        if (input == null || !input.equalsIgnoreCase("y")) {
            System.out.println("aborting");
            System.exit(1);
        }
    }
    if (!(new File(inFileName.value)).exists()) {
        System.out.println("Error: mesh file " + inFileName.value + " not found");
        System.exit(1);
    }
    GenericMeshReader reader = null;
    try {
        // File meshFile = new File (inFileName.value);
        reader = new GenericMeshReader(inFileName.value);
        if (className.value != null) {
            Class meshClass = Class.forName(className.value);
            Class meshBaseClass = new PolygonalMesh().getClass();
            if (meshClass == null) {
                System.out.println("can't find class " + className.value);
                System.exit(1);
            }
            if (!meshBaseClass.isAssignableFrom(meshClass)) {
                System.out.println(className.value + " is not an instance of " + meshBaseClass.getName());
                System.exit(1);
            }
            Constructor constructor = meshClass.getDeclaredConstructor(new Class[] {});
            if (constructor == null) {
                System.out.println("can't find constructor " + className.value + "()");
                System.exit(1);
            }
            mesh = (PolygonalMesh) constructor.newInstance(new Object[] {});
        } else {
            mesh = null;
        }
        mesh = reader.readMesh(mesh);
    // mesh.read (
    // new BufferedReader (new FileReader (meshFile)),
    // zeroIndexedIn.value);
    } catch (Exception e) {
        System.out.println("Error creating mesh object");
        e.printStackTrace();
        System.exit(1);
    }
    PolygonalMesh polyMesh = null;
    if (mesh instanceof PolygonalMesh) {
        polyMesh = (PolygonalMesh) mesh;
    }
    if (removeDisconnectedVertices.value) {
        if (polyMesh == null) {
            System.out.println("Not a polygonal mesh; ignoring -removeDisconnectedVertices");
        } else {
            System.out.println("removing disconnected vertices ...");
            int nv = polyMesh.removeDisconnectedVertices();
            if (nv > 0) {
                System.out.println(" removed " + nv);
            }
            System.out.println("done");
        }
    }
    if (closeVertexTol.value >= 0) {
        if (polyMesh == null) {
            System.out.println("Not a polygonal mesh; ignoring -mergeCloseVertices");
        } else {
            double rad = polyMesh.computeRadius();
            double dist = rad * closeVertexTol.value;
            System.out.println("removing vertices within " + dist + " ...");
            int nv = polyMesh.mergeCloseVertices(dist);
            if (nv > 0) {
                System.out.println(" removed " + nv);
            }
            polyMesh.checkForDegenerateFaces();
            System.out.println("done");
        }
    }
    if (removeDisconnectedFaces.value) {
        if (polyMesh == null) {
            System.out.println("Not a polygonal mesh; ignoring -removeDisconnectedFaces");
        } else {
            System.out.println("removing disconnected faces ...");
            int nf = polyMesh.removeDisconnectedFaces();
            if (nf > 0) {
                System.out.println(" removed " + nf);
            }
            System.out.println("done");
        }
    }
    if (laplacianSmoothing[0] != 0) {
        if (polyMesh == null) {
            System.out.println("Not a polygonal mesh; ignoring -laplacianSmooth");
        } else {
            System.out.println("smoothing ...");
            int iter = (int) laplacianSmoothing[0];
            double lam = laplacianSmoothing[1];
            double mu = laplacianSmoothing[2];
            LaplacianSmoother.smooth(polyMesh, iter, lam, mu);
            System.out.println("done");
        }
    }
    if (polyMesh != null && !polyMesh.isClosed()) {
        System.out.println("WARNING: mesh is not closed");
    }
    boolean translate = false;
    boolean rotate = false;
    if (xyz[0] != 0 || xyz[1] != 0 || xyz[2] != 0) {
        translate = true;
    }
    if (axisAngle[3] != 0) {
        rotate = true;
    }
    axisAngle[3] = Math.toRadians(axisAngle[3]);
    AffineTransform3dBase X = null;
    if (scaling.value != 1 || scaleXyz[0] != 1 || scaleXyz[1] != 1 || scaleXyz[2] != 1) {
        AffineTransform3d A = new AffineTransform3d();
        X = A;
        X.setTranslation(new Point3d(xyz));
        X.setRotation(new AxisAngle(axisAngle));
        A.applyScaling(scaleXyz[0] * scaling.value, scaleXyz[1] * scaling.value, scaleXyz[2] * scaling.value);
    } else if (translate || rotate) {
        X = new RigidTransform3d();
        X.setTranslation(new Point3d(xyz));
        X.setRotation(new AxisAngle(axisAngle));
    }
    boolean facesClockwise = false;
    if (X != null) {
        mesh.transform(X);
        if (X.getMatrix().determinant() < 0) {
            // then mesh has been flipped and we need to flip face ordering
            facesClockwise = true;
        }
    }
    try {
        GenericMeshWriter writer = new GenericMeshWriter(outFileName.value);
        writer.setFormat(reader);
        writer.writeMesh(mesh);
    } catch (Exception e) {
        e.printStackTrace();
        System.exit(1);
    }
}

Example 42 with AffineTransform3d

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

the class RigidBody method createFromMesh.

public static RigidBody createFromMesh(String bodyName, PolygonalMesh mesh, String meshFilePath, double density, double scale) {
    RigidBody body = new RigidBody(bodyName);
    // just to be sure ...
    mesh.triangulate();
    mesh.scale(scale);
    AffineTransform3d X = null;
    if (scale != 1) {
        X = AffineTransform3d.createScaling(scale);
    }
    body.setDensity(density);
    body.setMesh(mesh, meshFilePath, X);
    return body;
}

Example 43 with AffineTransform3d

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

the class CPD method affine.

/**
 * Uses the affine CPD algorithm to align a set of points
 * @param X reference input points
 * @param Y points to register
 * @param w weight, accounting to noise (w=0 --> no noise)
 * @param tol will iterative until objective function changes by less than this
 * @param maxIters maximum number of iterations
 * @param TY transformed points
 * @param trans initial guess of scaled rigid transform
 * @param sigma2Holder initial guess of variance
 * @return the scaled rigid transform for registration
 */
public static AffineTransform3d affine(Point3d[] X, Point3d[] Y, double w, double tol, int maxIters, Point3d[] TY, AffineTransform3d trans, double[] sigma2Holder) {
    int M = Y.length;
    int N = X.length;
    SVDecomposition3d svd = new SVDecomposition3d();
    if (trans == null) {
        trans = new AffineTransform3d();
        transformPoints(Y, TY);
    } else {
        transformPoints(Y, trans, TY);
    }
    double sigma2;
    if (sigma2Holder == null || sigma2Holder[0] < 0) {
        sigma2 = computeVariance(X, TY, null, 1.0 / M);
    } else {
        sigma2 = sigma2Holder[0];
    }
    Matrix3d B = new Matrix3d(trans.A);
    Vector3d t = new Vector3d(trans.p);
    double[][] P = new double[M][N];
    double[] P1 = new double[M];
    double[] Pt1 = new double[N];
    double Np;
    Matrix3d A = new Matrix3d();
    Matrix3d D = new Matrix3d();
    Matrix3d YPY = new Matrix3d();
    double[] tr = new double[2];
    Point3d meanx = new Point3d();
    Point3d meany = new Point3d();
    double err = Double.MAX_VALUE;
    int iters = 0;
    double q, qprev;
    q = Double.MAX_VALUE;
    // iterative part of algorithm
    while ((iters < maxIters) && (err > tol)) {
        // E-step
        Np = computeP(X, TY, sigma2, w, P, P1, Pt1);
        // M-step
        // mean
        computeMean(X, Pt1, Np, meanx);
        computeMean(Y, P1, Np, meany);
        // A = (X-mean(X))'*P'*(Y-mean(Y))
        // D = (Y-mean(Y))'*diag(P1)*(Y-mean(Y))
        computeAD(X, meanx, P, P1, Pt1, Y, meany, A, YPY, tr);
        // B = A*inverse(D)
        svd.factor(YPY);
        // pseudo-invert
        svd.pseudoInverse(D);
        B.mul(A, D);
        // t = mean(X)-A*mean(Y)
        t.mul(B, meany);
        t.sub(meanx, t);
        // Adjust output
        transformPoints(Y, B, t, TY);
        // speedy compute Q and sigma2
        A.mulTranspose(B);
        double trABt = A.trace();
        YPY.mulTranspose(B);
        YPY.mul(B);
        double trBYPYB = YPY.trace();
        // compute new objective (speedy)
        qprev = q;
        // q = computeQ(X, TY, P, Np, sigma2);
        q = (tr[0] - 2 * trABt + trBYPYB) / (2 * sigma2) + 1.5 * Np * Math.log(sigma2);
        // new variance estimate (speedy)
        // sigma2 = computeVariance(X, TY, P, Np);
        sigma2 = (tr[0] - trABt) / (3 * Np);
        if (sigma2 <= 0) {
            sigma2 = tol;
        }
        err = Math.abs(q - qprev);
        iters++;
    }
    if (verbose) {
        System.out.println("Registration complete in " + iters + " iterations");
    }
    // copy info over
    trans.A.set(B);
    trans.p.set(t);
    if (sigma2Holder != null) {
        sigma2Holder[0] = sigma2;
    }
    return trans;
}

Example 44 with AffineTransform3d

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

the class FemMuscleArm method addFemMuscle.

public void addFemMuscle(Point3d upper, Point3d lower) throws IOException {
    upperArm = model.rigidBodies().get("upper");
    RigidBody lowerArm = model.rigidBodies().get("lower");
    if (upperArm == null || lowerArm == null) {
        return;
    }
    muscle = new PointToPointMuscle("muscle", 1.0, 0.85, "muscle", true);
    model.addModel(muscle);
    RigidTransform3d X = new RigidTransform3d();
    // Point3d upper = new Point3d(2.4, 0, 20), lower = new Point3d(3, 0, -7);
    FemNode3d first = muscle.getFirstNode(), last = muscle.getLastNode();
    double scale = (upper.distance(lower) / first.getPosition().distance(last.getPosition()));
    System.out.println(scale);
    AffineTransform3d scaling = new AffineTransform3d();
    scaling.setIdentity();
    scaling.applyScaling(scale, scale, scale);
    // muscle.transformGeometry(scaling);
    Vector3d translate = new Vector3d();
    translate.sub(upper, first.getPosition());
    muscle.transformGeometry(new RigidTransform3d(translate, new AxisAngle()));
    Vector3d offfem = new Vector3d(), offtarget = new Vector3d();
    offfem.sub(last.getPosition(), first.getPosition());
    offtarget.sub(lower, upper);
    double angle = offfem.angle(offtarget);
    offfem.cross(offtarget);
    muscle.transformGeometry(new RigidTransform3d(new Vector3d(-upper.x, -upper.y, -upper.z), new AxisAngle()));
    muscle.transformGeometry(new RigidTransform3d(upper, new AxisAngle(offfem, angle)));
    muscle.getFirstNode().setDynamic(false);
    // ant.getFirstNode().setPosition(new Point3d(2.381, 0.000, 20));
    // model.AttachPoint(ant.getFirstNode(),upperArm,new Point3d(-size.x,0,-(size.z/2.0)/1.2));
    model.attachPoint(muscle.getLastNode(), lowerArm);
    muscle.setMaxForce(300000);
    muscle.setExcitation(0.0);
    RenderProps rp = new RenderProps(model.getRenderProps());
    rp.setFaceColor(Color.RED);
    rp.setLineStyle(Renderer.LineStyle.LINE);
    rp.setPointStyle(Renderer.PointStyle.POINT);
    rp.setShading(Renderer.Shading.SMOOTH);
    rp.setLineColor(Color.WHITE);
    muscle.setRenderProps(rp);
    muscle.setSurfaceRendering(SurfaceRender.Shaded);
}

Example 45 with AffineTransform3d

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

the class DicomImage method getVoxelTransform.

/**
 * Transform for converting integer voxel locations into spatial locations
 * @return the 3D affine transform for converting voxels to spatial locations
 */
public AffineTransform3d getVoxelTransform() {
    int nSlices = getNumSlices();
    // (slice thickness is sometimes different)
    if (nSlices > 1) {
        double zsize = slices[nSlices - 1].info.imagePosition.p.distance(slices[0].info.imagePosition.p);
        pixelSpacingSlice = zsize / (nSlices - 1);
    }
    if (pixelSpacingSlice == 0) {
        pixelSpacingSlice = slices[0].info.pixelSpacingSlice;
    }
    AffineTransform3d pixelTrans = new AffineTransform3d(trans);
    pixelTrans.applyScaling(pixelSpacingRows, pixelSpacingCols, pixelSpacingSlice);
    return pixelTrans;
}