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

Example 96 with RigidTransform3d

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

the class IntersectionTester method prerender.

@Override
public void prerender(RenderList list) {
    super.prerender(list);
    if (myZPerturb0 != null) {
        for (Vertex3d v : myMesh0.getVertices()) {
            if (isInterior(v)) {
                v.pnt.z = RandomGenerator.nextDouble(myZPerturb0.getLowerBound(), myZPerturb0.getUpperBound());
            }
        }
        myMesh0.notifyVertexPositionsModified();
        myMesh0.updateFaceNormals();
    }
    if (myZPerturb1 != null) {
        for (Vertex3d v : myMesh1.getVertices()) {
            if (isInterior(v)) {
                v.pnt.z = RandomGenerator.nextDouble(myZPerturb1.getLowerBound(), myZPerturb1.getUpperBound());
            }
        }
        myMesh1.notifyVertexPositionsModified();
        myMesh1.updateFaceNormals();
    }
    PolygonalMesh csgMesh = null;
    FunctionTimer timer = new FunctionTimer();
    if (myContoursOnly) {
        timer.start();
        System.out.println("finding contours");
        myContours = myIntersector.findContours(myMesh0, myMesh1);
        System.out.println("num contours=" + myContours.size());
        timer.stop();
        if (myContours.size() == 0) {
            return;
        }
    } else {
        timer.start();
        if (myCSGOperation == SurfaceMeshIntersector.CSG.INTERSECTION) {
            myContactInfo = myIntersector.findContoursAndRegions(myMesh0, RegionType.INSIDE, myMesh1, RegionType.INSIDE);
            csgMesh = myIntersector.createCSGMesh(myContactInfo);
        } else if (myCSGOperation == SurfaceMeshIntersector.CSG.UNION) {
            myContactInfo = myIntersector.findContoursAndRegions(myMesh0, RegionType.OUTSIDE, myMesh1, RegionType.OUTSIDE);
            csgMesh = myIntersector.createCSGMesh(myContactInfo);
        } else if (myCSGOperation == SurfaceMeshIntersector.CSG.DIFFERENCE01) {
            myContactInfo = myIntersector.findContoursAndRegions(myMesh0, RegionType.OUTSIDE, myMesh1, RegionType.INSIDE);
            csgMesh = myIntersector.createCSGMesh(myContactInfo);
        } else if (myCSGOperation == SurfaceMeshIntersector.CSG.DIFFERENCE10) {
            myContactInfo = myIntersector.findContoursAndRegions(myMesh0, RegionType.INSIDE, myMesh1, RegionType.OUTSIDE);
            csgMesh = myIntersector.createCSGMesh(myContactInfo);
        } else {
            myContactInfo = myIntersector.findContoursAndRegions(myMesh0, RegionType.INSIDE, myMesh1, RegionType.INSIDE);
        }
        timer.stop();
        if (myContactInfo == null) {
            myContours = null;
            return;
        }
        myContours = myContactInfo.getContours();
        ArrayList<PenetratingPoint> points0 = myContactInfo.getPenetratingPoints(0);
        // if (points0 != null) {
        // System.out.println ("num verts0= " + points0.size());
        // }
        // else {
        // System.out.println ("num verts0= null");
        // }
        ArrayList<PenetratingPoint> points1 = myContactInfo.getPenetratingPoints(1);
        // if (points1 != null) {
        // System.out.println ("num verts1= " + points1.size());
        // }
        // else {
        // System.out.println ("num verts1= null");
        // }
        ArrayList<PenetrationRegion> regions;
        if (false) {
            System.out.println("Regions 0:");
            regions = myContactInfo.getRegions(0);
            for (int i = 0; i < regions.size(); i++) {
                PenetrationRegion r = regions.get(i);
                int[] faceIdxs = SurfaceMeshIntersectorTest.getFaceIndices(r.getFaces());
                ArraySort.sort(faceIdxs);
                int[] vtxIdxs = SurfaceMeshIntersectorTest.getVertexIndices(r);
                ArraySort.sort(vtxIdxs);
                System.out.println(" " + i + " area=" + r.getArea() + " faces=[ " + ArraySupport.toString(faceIdxs) + " vtxs=[ " + ArraySupport.toString(vtxIdxs) + "]");
            }
            System.out.println("Regions 1:");
            regions = myContactInfo.getRegions(1);
            for (int i = 0; i < regions.size(); i++) {
                PenetrationRegion r = regions.get(i);
                int[] faceIdxs = SurfaceMeshIntersectorTest.getFaceIndices(r.getFaces());
                ArraySort.sort(faceIdxs);
                int[] vtxIdxs = SurfaceMeshIntersectorTest.getVertexIndices(r);
                ArraySort.sort(vtxIdxs);
                System.out.println(" " + i + " area=" + r.getArea() + " faces=[ " + ArraySupport.toString(faceIdxs) + " vtxs=[ " + ArraySupport.toString(vtxIdxs) + "]");
            }
            System.out.println("contours:");
            RigidTransform3d T = new RigidTransform3d();
            ArrayList<IntersectionContour> contours = myContactInfo.getContours();
            for (int k = 0; k < contours.size(); k++) {
                IntersectionContour c = contours.get(k);
                c.printCornerPoints("contour " + k + " closed=" + c.isClosed(), "%20.16f", T);
            }
        }
        if (false) {
            String pfx = "         ";
            System.out.print(pfx + "new int[] {");
            int k = 0;
            Collections.sort(points0, new CPPVertexComparator());
            for (PenetratingPoint cpp : points0) {
                System.out.print((k++ > 0 ? ", " : " ") + cpp.vertex.getIndex());
            }
            System.out.println(" },");
            System.out.print(pfx + "new int[] {");
            k = 0;
            Collections.sort(points1, new CPPVertexComparator());
            for (PenetratingPoint cpp : points1) {
                System.out.print((k++ > 0 ? ", " : " ") + cpp.vertex.getIndex());
            }
            System.out.println(" },");
        }
        if (myRenderCSGMesh && csgMesh != null) {
            csgMesh.prerender(myRenderProps);
            myCSGMesh = csgMesh;
        } else {
            myCSGMesh = null;
        }
    }
// System.out.println ("time=" + timer.result(1));
}

Example 97 with RigidTransform3d

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

the class LinearAuxiliaryTest method build.

@Override
public void build(String[] args) throws IOException {
    super.build(args);
    MechModel mech = new MechModel("mech");
    addModel(mech);
    double h = 0.1;
    double r = 0.005;
    LinearMaterial lmat = new LinearMaterial(50000, 0.45, true);
    FemModel3d fem1 = createCylinder(h, r);
    fem1.setName("auxiliary");
    mech.addModel(fem1);
    fem1.setMaterial(new NullMaterial());
    AuxMaterialBundle bundle = new AuxMaterialBundle("mat");
    for (FemElement3d elem : fem1.getElements()) {
        bundle.addElement(new AuxMaterialElementDesc(elem));
    }
    fem1.addMaterialBundle(bundle);
    bundle.setMaterial(lmat);
    FemModel3d fem2 = createCylinder(h, r);
    fem2.setName("linear");
    fem2.setMaterial(lmat);
    mech.addModel(fem2);
    RigidTransform3d rot = new RigidTransform3d(Vector3d.ZERO, AxisAngle.ROT_Y_90);
    fem1.transformGeometry(rot);
    fem2.transformGeometry(rot);
    fem2.transformGeometry(new RigidTransform3d(new Vector3d(0, 2 * r, 0), AxisAngle.IDENTITY));
    RenderProps.setFaceColor(fem2, Color.MAGENTA);
    addMonitor(new StiffnessErrorMonitor(fem1, fem2));
}

Example 98 with RigidTransform3d

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

the class ForceTargetDemo method build.

public void build(String[] args) throws IOException {
    // create MechModel and add to RootModel
    mech = new MechModel("mech");
    addModel(mech);
    // create the components
    p1 = new Particle("p1", /*mass=*/
    2, /*x,y,z=*/
    0, 0, 0.15);
    p2 = new Particle("p2", 2, 10, 0, 0.15);
    p3 = new Particle("p3", 2, 5, 5, 0);
    p4 = new Particle("p4", 2, 5, -5, 0);
    p5 = new Particle("p5", 0, 5, 0, 5);
    p6 = new Particle("p6", 0, 5, 0, -5);
    box = RigidBody.createBox("box", /*wx,wy,wz=*/
    0.5, 0.3, 0.3, /*density=*/
    1);
    box.setPose(new RigidTransform3d(/*x,y,z=*/
    5, 0, -0.15));
    // create marker point and connect it to the box:
    FrameMarker mkr = new FrameMarker(/*x,y,z=*/
    0, 0, 0.15);
    mkr.setFrame(box);
    // create the muscle:
    muscle = new Muscle("mus1", /*restLength=*/
    5);
    muscle.setPoints(p1, mkr);
    muscle.setMaterial(new SimpleAxialMuscle(/*stiffness=*/
    0, /*damping=*/
    10, /*maxf=*/
    1000));
    muscle2 = new Muscle("mus2", /*restLength=*/
    5);
    muscle2.setPoints(p2, mkr);
    muscle2.setMaterial(new SimpleAxialMuscle(/*stiffness=*/
    0, /*damping=*/
    10, /*maxf=*/
    1000));
    muscle3 = new Muscle("mus3", /*restLength=*/
    5);
    muscle3.setPoints(p3, mkr);
    muscle3.setMaterial(new SimpleAxialMuscle(/*stiffness=*/
    0, /*damping=*/
    10, /*maxf=*/
    4000));
    muscle4 = new Muscle("mus4", /*restLength=*/
    5);
    muscle4.setPoints(p4, mkr);
    muscle4.setMaterial(new SimpleAxialMuscle(/*stiffness=*/
    0, /*damping=*/
    10, /*maxf=*/
    4000));
    muscle5 = new Muscle("mus5", /*restLength=*/
    5);
    muscle5.setPoints(p5, mkr);
    muscle5.setMaterial(new SimpleAxialMuscle(/*stiffness=*/
    0, /*damping=*/
    10, /*maxf=*/
    4000));
    muscle6 = new Muscle("mus6", /*restLength=*/
    5);
    muscle6.setPoints(p6, mkr);
    muscle6.setMaterial(new SimpleAxialMuscle(/*stiffness=*/
    0, /*damping=*/
    10, /*maxf=*/
    4000));
    if (two_cons == true) {
        RigidTransform3d XPW = new RigidTransform3d(5, 0, 0);
        XPW.R.mulAxisAngle(1, 0, 0, Math.toRadians(90));
        // Connection on the corner
        // PlanarConnector connector =
        // new PlanarConnector (box, new Vector3d (lenx/2, -2.5, 1.5), XPW);
        // Connection in the center
        con = new PlanarConnector(box, new Vector3d(0, 0, 0.15), XPW);
        con.setUnilateral(false);
        con.setPlaneSize(2);
        RenderProps props = con.createRenderProps();
        props.setPointColor(Color.blue);
        props.setPointStyle(Renderer.PointStyle.SPHERE);
        props.setPointRadius(0.06);
        con.setRenderProps(props);
        // con = new ParticlePlaneConstraint(p5, pl);
        // RenderProps.setDrawEdges (con, true);
        // RenderProps.setVisible (con, true);
        RigidTransform3d XPW2 = new RigidTransform3d(5, 0, 0);
        XPW2.R.mulAxisAngle(1, 0, 0, 0);
        // Connection on the corner
        // PlanarConnector connector =
        // new PlanarConnector (box, new Vector3d (lenx/2, -2.5, 1.5), XPW);
        // Connection in the center
        con2 = new PlanarConnector(box, new Vector3d(0, 0, 0.15), XPW2);
        con2.setUnilateral(false);
        con2.setPlaneSize(2);
        con2.setRenderProps(props);
    } else {
        RigidTransform3d XPW = new RigidTransform3d(5, 0, 0);
        XPW.R.mulAxisAngle(1, 0, 0, Math.toRadians(45));
        con = new PlanarConnector(box, new Vector3d(0, 0, 0.15), XPW);
        con.setUnilateral(false);
        con.setPlaneSize(2);
        RenderProps props = con.createRenderProps();
        props.setPointColor(Color.blue);
        props.setPointStyle(Renderer.PointStyle.SPHERE);
        props.setPointRadius(0.06);
        con.setRenderProps(props);
    }
    // con.setRenderProps (myRenderProps);
    // add components to the mech model
    mech.addParticle(p1);
    mech.addParticle(p2);
    mech.addParticle(p3);
    mech.addParticle(p4);
    mech.addParticle(p5);
    mech.addParticle(p6);
    mech.addRigidBody(box);
    mech.addFrameMarker(mkr);
    // mech.addParticle (p5);
    mech.addAxialSpring(muscle);
    mech.addAxialSpring(muscle2);
    mech.addAxialSpring(muscle3);
    mech.addAxialSpring(muscle4);
    mech.addAxialSpring(muscle5);
    mech.addAxialSpring(muscle6);
    con.setName("con1");
    if (two_cons == true) {
        con2.setName("con2");
    }
    if (cons == true) {
        mech.addBodyConnector(con);
        ConnectorForceRenderer rend = new ConnectorForceRenderer(con);
        myRenderProps = rend.createRenderProps();
        myRenderProps.setLineStyle(LineStyle.CYLINDER);
        myRenderProps.setLineRadius(0.175);
        myRenderProps.setLineColor(Color.BLUE);
        rend.setRenderProps(myRenderProps);
        rend.setArrowSize(0.1);
        addMonitor(rend);
    }
    if (two_cons == true) {
        mech.addBodyConnector(con2);
    }
    // first particle set to be fixed
    p1.setDynamic(false);
    p2.setDynamic(false);
    p3.setDynamic(false);
    p4.setDynamic(false);
    p5.setDynamic(false);
    p6.setDynamic(false);
    // increase model bounding box for the viewer
    mech.setBounds(/*min=*/
    -1, 0, -1, /*max=*/
    1, 0, 0);
    // set render properties for the components
    setPointRenderProps(p1);
    setPointRenderProps(p2);
    setPointRenderProps(p3);
    setPointRenderProps(p4);
    setPointRenderProps(p5);
    setPointRenderProps(p6);
    setPointRenderProps(mkr);
    setLineRenderProps(muscle);
    setLineRenderProps(muscle2);
    setLineRenderProps(muscle3);
    setLineRenderProps(muscle4);
    setLineRenderProps(muscle5);
    setLineRenderProps(muscle6);
    addTrackingController(mkr);
    if (cons = true) {
        addConForceProbe(10, 0.1);
    }
}

Example 99 with RigidTransform3d

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

the class PointModel method addFullMuscles.

public void addFullMuscles() {
    RigidTransform3d X = new RigidTransform3d();
    int num = 2;
    // x-z plane
    addMuscles(X, num, len / 2.0);
    X.R.setAxisAngle(1, 0, 0, Math.PI / 2.0);
    addMuscles(X, num, 0.0);
    X.R.setAxisAngle(0, 1, 0, Math.PI / 2.0);
    addMuscles(X, num, -len / 2.0);
}

Example 100 with RigidTransform3d

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

the class SimpleCollide method setBottomObject.

private void setBottomObject(Collidable comp, ObjectType type) {
    Renderable rcomp = (Renderable) comp;
    ((TransformableGeometry) comp).transformGeometry(AffineTransform3d.createScaling(myBottomScale));
    switch(type) {
        case FemEllipsoid:
        case FemSphere:
            {
                RenderProps.setPointColor(rcomp, Color.green);
                // fix the lower nodes
                Point3d min = new Point3d();
                RenderableUtils.getBounds(rcomp, min, null);
                FemModel3d fem = (FemModel3d) comp;
                fem.getSurfaceMesh();
                for (FemNode3d n : fem.getNodes()) {
                    if (fem.isSurfaceNode(n) && n.getPosition().z <= (min.z + mySize * 0.5)) {
                        n.setDynamic(false);
                    }
                }
                fem.resetRestPosition();
                break;
            }
        case FemCube:
            {
                RenderProps.setPointColor(rcomp, Color.green);
                // fix the lower nodes
                Point3d min = new Point3d();
                RenderableUtils.getBounds(rcomp, min, null);
                FemModel3d fem = (FemModel3d) comp;
                fem.getSurfaceMesh();
                for (FemNode3d n : fem.getNodes()) {
                    if (fem.isSurfaceNode(n) && n.getPosition().z <= (min.z + mySize * 0.1)) {
                        n.setDynamic(false);
                    }
                }
                // RenderProps.setAlpha (rcomp, 0.3);
                fem.resetRestPosition();
                break;
            }
        case Box:
            {
                ((RigidBody) comp).setDynamic(false);
                ((RigidBody) comp).setDistanceGridRes(new Vector3i(10, 5, 5));
                break;
            }
        case Molar:
            {
                ((RigidBody) comp).setPose(new RigidTransform3d(0, 0, 0, 0, -1, 0, Math.toRadians(172.09)));
                ((RigidBody) comp).setDynamic(false);
                break;
            }
        case Bin:
            {
                ((RigidBody) comp).setPose(new RigidTransform3d(0, 0, 0, 0, 0, 0, 0));
                ((RigidBody) comp).setDynamic(false);
                break;
            }
        case Paw:
        case House:
            {
                ((RigidBody) comp).setDynamic(false);
                break;
            }
        default:
            {
                throw new InternalErrorException("Unimplemented type " + type);
            }
    }
    myBottomObject = comp;
    myBottomType = type;
}