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

Example 31 with AxisAngle

use of maspack.matrix.AxisAngle 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 32 with AxisAngle

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

the class MeshColliderTest method testEdgeEdge.

boolean testEdgeEdge() {
    PolygonalMesh mesh0 = MeshFactory.createBox(1, 1, 1);
    PolygonalMesh mesh1 = MeshFactory.createBox(1, 1, 1);
    RigidTransform3d trans0 = new RigidTransform3d(new Vector3d(1, 0, 1), new AxisAngle());
    mesh0.setMeshToWorld(trans0);
    RigidTransform3d trans1 = new RigidTransform3d();
    trans1.mulAxisAngle(new AxisAngle(0, 1, 0, Math.PI / 4));
    trans1.mulAxisAngle(new AxisAngle(1, 0, 0, Math.PI / 4));
    mesh1.setMeshToWorld(trans1);
    MeshCollider collider = new MeshCollider();
    // first way
    ContactInfo info = collider.getContacts(mesh0, mesh1);
    if (info == null) {
        return false;
    }
    ArrayList<ContactPlane> regions = info.getContactPlanes();
    if (regions.size() != 1)
        return false;
    ContactPlane region = regions.get(0);
    if (region.points.size() != 1)
        return false;
    if (!region.points.get(0).epsilonEquals(new Vector3d(0.5, 0, 0.5), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(1.0 / Math.sqrt(2.0), 0, 1.0 / Math.sqrt(2.0)), epsilon))
        return false;
    // second way
    info = collider.getContacts(mesh1, mesh0);
    if (info == null)
        return false;
    if (regions.size() != 1)
        return false;
    region = regions.get(0);
    if (region.points.size() != 1)
        return false;
    if (!region.points.get(0).epsilonEquals(new Vector3d(0.5, 0, 0.5), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(-1.0 / Math.sqrt(2.0), 0, -1.0 / Math.sqrt(2.0)), epsilon))
        return false;
    return true;
}

Example 33 with AxisAngle

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

the class MeshColliderTest method testVertexFace.

boolean testVertexFace() {
    PolygonalMesh mesh0 = MeshFactory.createBox(1, 1, 1);
    PolygonalMesh mesh1 = MeshFactory.createBox(1, 1, 1);
    RigidTransform3d trans0 = new RigidTransform3d(new Vector3d(Math.sqrt(3.0) / 2.0 + 0.5, 0, 0.25), new AxisAngle());
    mesh0.setMeshToWorld(trans0);
    RigidTransform3d trans1 = new RigidTransform3d();
    trans1.mulAxisAngle(new AxisAngle(0, 1, 0, Math.atan(Math.sqrt(2))));
    trans1.mulAxisAngle(new AxisAngle(1, 0, 0, Math.PI / 4));
    mesh1.setMeshToWorld(trans1);
    MeshCollider collider = new MeshCollider();
    // first way
    ContactInfo info = collider.getContacts(mesh0, mesh1);
    if (info == null) {
        return false;
    }
    ArrayList<ContactPlane> regions = info.getContactPlanes();
    if (regions.size() != 1)
        return false;
    ContactPlane region = regions.get(0);
    if (region.points.size() != 1)
        return false;
    if (!region.points.get(0).epsilonEquals(new Vector3d(Math.sqrt(3.0 / 4.0), 0, 0), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(1, 0, 0), epsilon))
        return false;
    // first way
    info = collider.getContacts(mesh1, mesh0);
    if (info == null)
        return false;
    if (regions.size() != 1)
        return false;
    region = regions.get(0);
    if (region.points.size() != 1)
        return false;
    if (!region.points.get(0).epsilonEquals(new Vector3d(Math.sqrt(3.0 / 4.0), 0, 0), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(-1, 0, 0), epsilon))
        return false;
    return true;
}

Example 34 with AxisAngle

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

the class MeshColliderTest method testVertexEdge.

boolean testVertexEdge() {
    PolygonalMesh mesh0 = MeshFactory.createBox(1, 1, 1);
    PolygonalMesh mesh1 = MeshFactory.createBox(1, 1, 1);
    RigidTransform3d trans0 = new RigidTransform3d(new Vector3d(Math.sqrt(3.0) / 2.0 + 0.5, 0, 0.5), new AxisAngle());
    mesh0.setMeshToWorld(trans0);
    RigidTransform3d trans1 = new RigidTransform3d();
    trans1.mulAxisAngle(new AxisAngle(0, 1, 0, Math.atan(Math.sqrt(2))));
    trans1.mulAxisAngle(new AxisAngle(1, 0, 0, Math.PI / 4));
    mesh1.setMeshToWorld(trans1);
    MeshCollider collider = new MeshCollider();
    // first way
    ContactInfo info = collider.getContacts(mesh0, mesh1);
    if (info == null) {
        return false;
    }
    ArrayList<ContactPlane> regions = info.getContactPlanes();
    if (regions.size() != 1)
        return false;
    ContactPlane region = regions.get(0);
    if (region.points.size() != 1)
        return false;
    if (!region.points.get(0).epsilonEquals(new Vector3d(Math.sqrt(3.0 / 4.0), 0, 0), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(1.0 / Math.sqrt(2), 0, 1.0 / Math.sqrt(2)), epsilon))
        return false;
    // second way
    info = collider.getContacts(mesh1, mesh0);
    if (info == null) {
        return false;
    }
    regions = info.getContactPlanes();
    if (regions.size() != 1)
        return false;
    region = regions.get(0);
    if (region.points.size() != 1)
        return false;
    if (!region.points.get(0).epsilonEquals(new Vector3d(Math.sqrt(3.0 / 4.0), 0, 0), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(-1.0 / Math.sqrt(2), 0, -1.0 / Math.sqrt(2)), epsilon))
        return false;
    return true;
}

Example 35 with AxisAngle

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

the class RotAxisFrameMaterial method computeDFdq.

public void computeDFdq(Matrix6d Jq, RigidTransform3d X21, Twist vel21, RigidTransform3d initialX21, boolean symmetric) {
    AxisAngle axisAng = new AxisAngle();
    X21.R.getAxisAngle(axisAng);
    Matrix3d U = new Matrix3d();
    Jq.setZero();
    computeU(U, axisAng, symmetric);
    Jq.m00 = myK.x;
    Jq.m11 = myK.y;
    Jq.m22 = myK.z;
    // set lower right submatrix to myRotK*U
    Jq.m33 = myRotK * U.m00;
    Jq.m34 = myRotK * U.m01;
    Jq.m35 = myRotK * U.m02;
    Jq.m43 = myRotK * U.m10;
    Jq.m44 = myRotK * U.m11;
    Jq.m45 = myRotK * U.m12;
    Jq.m53 = myRotK * U.m20;
    Jq.m54 = myRotK * U.m21;
    Jq.m55 = myRotK * U.m22;
}