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

Example 1 with RigidTransform3d

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

the class MeshThicken method drawToolEnd.

public void drawToolEnd(DrawToolEvent e) {
    System.out.println("Entered");
    Dragger3d tool = e.getSource();
    if (tool instanceof SplineTool) {
        SplineTool splineTool = (SplineTool) tool;
        NURBSCurve2d curve = splineTool.getCurve();
        if (curve != null) {
            if (myEditingRegion != null) {
                myEditingRegion.setCurve(new NURBSCurve2d(curve));
                myEditingRegion = null;
            } else {
                RigidTransform3d X = new RigidTransform3d();
                splineTool.getToolToWorld(X);
                X.mulXyz(0, 0, -myDefaultRegionHeight);
                Region region = new Region(curve, X, myDefaultRegionHeight);
                region.myMargin = myDefaultRegionMargin;
                region.myThickening = myDefaultRegionThickening;
                region.myUnthickening = myDefaultRegionUnthickening;
                addRegion(region);
            }
        }
        splineTool.clear();
        viewer.rerender();
    }
}

Example 2 with RigidTransform3d

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

the class MeshViewer method main.

public static void main(String[] args) {
    StringHolder fileName = new StringHolder();
    IntHolder width = new IntHolder(640);
    IntHolder height = new IntHolder(480);
    Vector meshFileList = new Vector();
    ArrayList<String> meshQueue = new ArrayList<String>();
    ArgParser parser = new ArgParser("java maspack.geometry.MeshViewer [options] <objFile> ...");
    // parser.addOption ("-file %s #mesh file names", meshFileList);
    parser.addOption("-width %d #width (pixels)", width);
    parser.addOption("-height %d #height (pixels)", height);
    parser.addOption("-drawAxes %v #draw coordinate axes", drawAxes);
    parser.addOption("-drawEdges %v #draw mesh edges", drawEdges);
    parser.addOption("-noDrawFaces %v #do not draw faces", noDrawFaces);
    parser.addOption("-edgeColor %fX3 #edge color", edgeColor);
    parser.addOption("-backgroundColor %fX3 #background color", backgroundColor);
    parser.addOption("-rotate %fX4 #rotation (axis-angle)", rotation);
    parser.addOption("-axisLength %f #coordinate axis length", axisLength);
    parser.addOption("-smooth %v #use smooth shading", smooth);
    parser.addOption("-oneSided %v #draw only front faces", oneSided);
    parser.addOption("-class %s #use PolygonalMesh sub class", className);
    parser.addOption("-zeroIndexed %v #zero indexed (for writing)", zeroIndexed);
    parser.addOption("-queue %v #queue meshes for viewing", queueMeshes);
    parser.addOption("-printBounds %v #print bounds for meshes", printBounds);
    parser.addOption("-skip %d #for .xyzb point meshes, use every n-th point", skipCount);
    parser.addOption("-pointMesh %v #specifies that mesh must be read as a point mesh", pointMesh);
    RigidTransform3d X = new RigidTransform3d();
    ArrayList<MeshInfo> infoList = new ArrayList<MeshInfo>(0);
    int idx = 0;
    while (idx < args.length) {
        try {
            idx = parser.matchArg(args, idx);
            if (parser.getUnmatchedArgument() != null) {
                String meshFileName = parser.getUnmatchedArgument();
                if (queueMeshes.value) {
                    meshQueue.add(meshFileName);
                } else {
                    X.R.setAxisAngle(rotation[0], rotation[1], rotation[2], Math.toRadians(rotation[3]));
                    loadMeshes(infoList, meshFileName, X);
                }
            }
        } catch (Exception e) {
            // malformed or erroneous argument
            parser.printErrorAndExit(e.getMessage());
        }
    }
    // parser.matchAllArgs (args);
    // for (Iterator it = meshFileList.iterator(); it.hasNext();) {
    // loadMeshes (meshList, ((StringHolder)it.next()).value, X);
    // }
    // call this to prevent awful looking fonts:
    System.setProperty("awt.useSystemAAFontSettings", "on");
    final MeshViewer frame = new MeshViewer("MeshViewer", infoList, width.value, height.value);
    frame.setMeshQueue(meshQueue);
    frame.setVisible(true);
    // add close
    frame.addWindowListener(new WindowListener() {

        @Override
        public void windowOpened(WindowEvent arg0) {
        }

        @Override
        public void windowIconified(WindowEvent arg0) {
        }

        @Override
        public void windowDeiconified(WindowEvent arg0) {
        }

        @Override
        public void windowDeactivated(WindowEvent arg0) {
        }

        @Override
        public void windowClosing(WindowEvent arg0) {
            frame.exit(0);
        }

        @Override
        public void windowClosed(WindowEvent arg0) {
        }

        @Override
        public void windowActivated(WindowEvent arg0) {
        }
    });
}

Example 3 with RigidTransform3d

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

the class MeshCollider method getContactPlaneInfo.

/**
 * Get information about a specific region of intersections.
 */
static void getContactPlaneInfo(ContactPlane region, PolygonalMesh mesh0, PolygonalMesh mesh1, double pointTol) {
    Vector3d sectnormal = new Vector3d(), tmpnormal = new Vector3d();
    BVFeatureQuery query = new BVFeatureQuery();
    RigidTransform3d trans0 = mesh0.getMeshToWorld();
    RigidTransform3d trans1 = mesh1.getMeshToWorld();
    // calculate a weighted average of the face normals
    for (TriTriIntersection isect : region.intersections) {
        region.points = new ArrayList<Point3d>();
        region.points.add(isect.points[0]);
        region.points.add(isect.points[1]);
        double length = isect.points[0].distance(isect.points[1]);
        tmpnormal.transform(trans0, isect.face0.getNormal());
        tmpnormal.negate();
        sectnormal.scaledAdd(length, tmpnormal, sectnormal);
        tmpnormal.transform(trans1, isect.face1.getNormal());
        sectnormal.scaledAdd(length, tmpnormal, sectnormal);
    }
    // calculate the weighted intersection center
    Point3d center = new Point3d();
    double weight = 0;
    for (TriTriIntersection isect : region.intersections) {
        double length = isect.points[0].distance(isect.points[1]);
        center.scaledAdd(length, isect.points[0], center);
        center.scaledAdd(length, isect.points[1], center);
        weight += 2 * length;
    }
    center.scale(1.0 / weight);
    region.centroid = center;
    // calculate the weighted normal
    Vector3d cp0 = new Vector3d(), cp1 = new Vector3d();
    region.normal.setZero();
    for (TriTriIntersection isect : region.intersections) {
        cp0.sub(isect.points[0], center);
        cp1.sub(isect.points[1], center);
        tmpnormal.cross(cp0, cp1);
        if (tmpnormal.dot(sectnormal) < 0)
            tmpnormal.negate();
        region.normal.add(tmpnormal);
    }
    if (region.normal.dot(sectnormal) < 0)
        region.normal.negate();
    // handle degenerate cases
    if (region.normal.containsNaN() || region.normal.norm() < EPS) {
        region.normal.setZero();
        Point3d p0 = new Point3d();
        Point3d p1 = new Point3d();
        Vector3d c0 = new Vector3d();
        Vector3d c1 = new Vector3d();
        for (TriTriIntersection isect : region.intersections) {
            for (Point3d p : isect.points) {
                p0.inverseTransform(trans0, p);
                p1.inverseTransform(trans1, p);
                Vertex3d u0 = isect.face0.getVertex(0);
                Vertex3d u1 = isect.face0.getVertex(1);
                Vertex3d u2 = isect.face0.getVertex(2);
                Vertex3d v0 = isect.face1.getVertex(0);
                Vertex3d v1 = isect.face1.getVertex(1);
                Vertex3d v2 = isect.face1.getVertex(2);
                getCoordinates(c0, u0.pnt, u1.pnt, u2.pnt, p0);
                getCoordinates(c1, v0.pnt, v1.pnt, v2.pnt, p1);
                int[] type0 = classifyPoint(c0);
                int[] type1 = classifyPoint(c1);
                if (type0[0] == 2) {
                    if (type1[0] == 2) {
                        // vertex,vertex
                        region.normal.add(vertexVertexNormal(trans0, trans1, isect.face0, isect.face1, type0[1], type1[1]));
                    } else if (type1[0] == 1) {
                        // vertex,edge
                        region.normal.add(vertexEdgeNormal(trans0, trans1, isect.face0, isect.face1, type0[1], type1[1]));
                    } else {
                        // vertex,face
                        region.normal.add(vertexFaceNormal(trans0, trans1, isect.face0, isect.face1, type0[1]));
                    }
                } else if (type0[0] == 1) {
                    if (type1[0] == 2) {
                        // edge,vertex
                        region.normal.sub(vertexEdgeNormal(trans1, trans0, isect.face1, isect.face0, type1[1], type0[1]));
                    } else if (type1[0] == 1) {
                        // edge,edge
                        region.normal.add(edgeEdgeNormal(trans0, trans1, isect.face0, isect.face1, type0[1], type1[1]));
                    } else {
                        // edge,face
                        region.normal.add(edgeFaceNormal(trans0, trans1, isect.face0, isect.face1, type0[1]));
                    }
                } else {
                    if (type1[0] == 2) {
                        // face,vertex
                        region.normal.sub(vertexFaceNormal(trans1, trans0, isect.face1, isect.face0, type1[1]));
                    } else if (type1[0] == 1) {
                        // face,edge
                        region.normal.sub(edgeFaceNormal(trans1, trans0, isect.face1, isect.face0, type1[1]));
                    } else {
                        // face,face
                        region.normal.add(faceFaceNormal(trans0, trans1, isect.face0, isect.face1));
                    }
                }
            }
        }
    }
    region.normal.normalize();
    // calculate the contact depth for the region
    boolean foundPenetratingVertice = false;
    Point3d p = new Point3d();
    Point3d nearest = new Point3d();
    Vector3d diff = new Vector3d();
    Vector2d coords = new Vector2d();
    Vertex3d v;
    Face nf;
    Point3d plocal = new Point3d();
    LinkedHashSet<Vertex3d> regionvertices0 = new LinkedHashSet<Vertex3d>();
    LinkedHashSet<Vertex3d> regionvertices1 = new LinkedHashSet<Vertex3d>();
    region.depth = 0;
    for (TriTriIntersection isect : region.intersections) {
        for (int i = 0; i < 3; i++) {
            // face0 vertex depths
            v = isect.face0.getVertex(i);
            p.transform(trans0, v.pnt);
            plocal.inverseTransform(trans1, p);
            plocal.sub(isect.face1.getVertex(0).pnt);
            if (plocal.dot(isect.face1.getNormal()) <= 0) {
                regionvertices0.add(v);
            }
            // face1 vertex depths
            v = isect.face1.getVertex(i);
            p.transform(trans1, v.pnt);
            plocal.inverseTransform(trans0, p);
            plocal.sub(isect.face0.getVertex(0).pnt);
            if (plocal.dot(isect.face0.getNormal()) <= 0) {
                regionvertices1.add(v);
            }
        }
    }
    for (Vertex3d v0 : regionvertices0) {
        p.transform(trans0, v0.pnt);
        // XXX Sanchez, Jun 22, 2014
        // Changed to isInside.  Sometimes a vertex is outside
        // the mesh but determined to be "penetrating" due to
        // normal (e.g. when nearest to an edge)
        // nf = myQuery.nearestFaceToPoint (nearest, coords, mesh1, p);
        boolean inside = query.isInsideOrientedMesh(mesh1, p, 0);
        if (inside) {
            query.getFaceForInsideOrientedTest(nearest, coords);
            nearest.transform(trans1);
            diff.sub(p, nearest);
            diff.inverseTransform(trans1);
            foundPenetratingVertice = true;
            // -diff.dot (nf.getNormal());
            double dist = diff.norm();
            if (dist > region.depth)
                region.depth = dist;
        }
    }
    for (Vertex3d v1 : regionvertices1) {
        p.transform(trans1, v1.pnt);
        // nf = myQuery.nearestFaceToPoint (nearest, coords, mesh0, p);
        boolean inside = query.isInsideOrientedMesh(mesh0, p, 0);
        if (inside) {
            query.getFaceForInsideOrientedTest(nearest, coords);
            nearest.transform(trans0);
            diff.sub(p, nearest);
            diff.inverseTransform(trans0);
            foundPenetratingVertice = true;
            // -diff.dot (nf.getNormal());
            double dist = diff.norm();
            if (dist > region.depth)
                region.depth = dist;
        }
    }
    if (!foundPenetratingVertice) {
        double min = Double.POSITIVE_INFINITY, max = Double.NEGATIVE_INFINITY;
        for (int i = 0; i < region.points.size(); i++) {
            double d = region.points.get(i).dot(region.normal);
            if (d < min)
                min = d;
            if (d > max)
                max = d;
        }
        region.depth = max - min;
    }
    // eliminate redundant points
    // use point tolerance
    region.points.clear();
    for (TriTriIntersection isect : region.intersections) {
        for (Point3d pcandidate : isect.points) {
            boolean add = true;
            for (Point3d other : region.points) if (pcandidate.epsilonEquals(other, pointTol)) {
                add = false;
                break;
            }
            if (add) {
                region.points.add(pcandidate);
            }
        }
    }
    // take extrema along n axes
    if (numextremaaxes > 0) {
        // final ArrayList<Vector3d> axes = new ArrayList<Vector3d>();
        Vector3d crosszup = new Vector3d(0, 0, 1);
        crosszup.cross(region.normal, crosszup);
        double crosszupnorm = crosszup.norm();
        RigidTransform3d normtoworld;
        if (crosszup.norm() > EPS) {
            normtoworld = new RigidTransform3d(new Vector3d(), new AxisAngle(crosszup, Math.asin(crosszupnorm)));
        } else {
            normtoworld = new RigidTransform3d();
        }
        boolean[] keep = new boolean[region.points.size()];
        for (int j = 0; j < region.points.size(); j++) keep[j] = false;
        Vector3d offset = new Vector3d();
        Vector3d axis = new Vector3d();
        for (int i = 0; i < numextremaaxes; i++) {
            double min = Double.POSITIVE_INFINITY, max = Double.NEGATIVE_INFINITY;
            int mini = 0, maxi = 0;
            double angle = Math.PI * i / numextremaaxes;
            axis.set(Math.cos(angle), Math.sin(angle), 0);
            axis.transform(normtoworld);
            for (int j = 0; j < region.points.size(); j++) {
                offset.sub(region.points.get(j), center);
                double dot = offset.dot(axis);
                if (dot < min) {
                    min = dot;
                    mini = j;
                }
                if (dot > max) {
                    max = dot;
                    maxi = j;
                }
            }
            keep[mini] = true;
            keep[maxi] = true;
        }
        for (int j = (region.points.size() - 1); j >= 0; j--) {
            if (!keep[j])
                region.points.remove(j);
        }
    }
}

Example 4 with RigidTransform3d

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

the class MeshColliderTest method testRegions.

boolean testRegions() {
    PolygonalMesh mesh0 = MeshFactory.createBox(1, 1, 1);
    PolygonalMesh mesh1 = MeshFactory.createBox(1, 1, 1);
    mesh0.scale(0.9);
    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;
    }
    if (info.getContactPlanes().size() != 6) {
        return false;
    }
    // second way
    info = collider.getContacts(mesh1, mesh0);
    if (info == null) {
        return false;
    }
    if (info.getContactPlanes().size() != 6) {
        return false;
    }
    return true;
}

Example 5 with RigidTransform3d

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

the class MeshColliderTest method testVertexVertex.

boolean testVertexVertex() {
    PolygonalMesh mesh0 = MeshFactory.createBox(1, 1, 1);
    PolygonalMesh mesh1 = MeshFactory.createBox(1, 1, 1);
    RigidTransform3d trans0 = new RigidTransform3d(new Vector3d(Math.sqrt(3), 0, 0), new AxisAngle());
    trans0.mulAxisAngle(new AxisAngle(0, 1, 0, Math.atan(Math.sqrt(2))));
    trans0.mulAxisAngle(new AxisAngle(1, 0, 0, Math.PI / 4));
    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) / 2.0, 0, 0), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(0.9994849337479609, 0, -0.03209154422638611), 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) / 2.0, 0, 0), epsilon))
        return false;
    if (!region.normal.epsilonEquals(new Vector3d(-0.9994849337479609, 0, 0.03209154422638611), epsilon))
        return false;
    return true;
}