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

Example 81 with RigidTransform3d

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

the class PointDistributor method sphereFCCFill.

// fills a mesh with spheres based on face-centered cubic packing
public static Point3d[] sphereFCCFill(PolygonalMesh mesh, double r) {
    ArrayList<Point3d> pnts = new ArrayList<Point3d>();
    RigidTransform3d trans = getPrincipalAxes(mesh);
    Point3d[] box = getTightBox(mesh, trans);
    Point3d center = new Point3d(box[0]);
    center.add(box[6]);
    center.scale(0.5);
    trans.setTranslation(center);
    Vector3d l = new Vector3d(box[0]);
    l.sub(box[6]);
    l.inverseTransform(trans);
    double alpha = 2 * Math.sqrt(2) * r;
    int nx = (int) Math.ceil(l.x / alpha) + 1;
    int ny = (int) Math.ceil(l.y / alpha) + 1;
    int nz = (int) Math.ceil(l.z / alpha) + 1;
    double xoffset = -(nx - 1) * alpha / 2;
    double yoffset = -(ny - 1) * alpha / 2;
    double zoffset = -(nz - 1) * alpha / 2;
    BVTree bvh = mesh.getBVTree();
    Vector2d coords = new Vector2d();
    Point3d nearest = new Point3d();
    BVFeatureQuery query = new BVFeatureQuery();
    Point3d p;
    for (int i = 0; i < nx; i++) {
        for (int j = 0; j < ny; j++) {
            for (int k = 0; k < nz; k++) {
                double x = i * alpha + xoffset;
                double y = j * alpha + yoffset;
                double z = k * alpha + zoffset;
                p = new Point3d(x, y, z);
                p.transform(trans);
                addIfIntersects(pnts, p, r, bvh, nearest, coords, query);
                // face centers
                if (i < nx - 1 && k < nz - 1) {
                    p = new Point3d(x + alpha / 2, y, z + alpha / 2);
                    p.transform(trans);
                    addIfIntersects(pnts, p, r, bvh, nearest, coords, query);
                }
                if (j < ny - 1 && k < nz - 1) {
                    p = new Point3d(x, y + alpha / 2, z + alpha / 2);
                    p.transform(trans);
                    addIfIntersects(pnts, p, r, bvh, nearest, coords, query);
                }
                if (i < nx - 1 && j < ny - 1) {
                    p = new Point3d(x + alpha / 2, y + alpha / 2, z);
                    p.transform(trans);
                    addIfIntersects(pnts, p, r, bvh, nearest, coords, query);
                }
            }
        }
    }
    return pnts.toArray(new Point3d[pnts.size()]);
}

Example 82 with RigidTransform3d

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

the class PointDistributor method getSphericalCapMidpointCubature.

public static CubaturePoint3d[] getSphericalCapMidpointCubature(Point3d center, double radius, double height, int nR, int nTheta, int nH, Vector3d axis) {
    CubaturePoint3d[] pnts = new CubaturePoint3d[nR * nTheta * nH];
    double dr = radius / nR;
    double dtheta = 2 * Math.PI / nTheta;
    double dh = height / nH;
    RigidTransform3d trans = new RigidTransform3d();
    if (axis != null) {
        RotationMatrix3d R = new RotationMatrix3d();
        R.rotateZDirection(axis);
        trans.setRotation(R);
    }
    Vector3d t = new Vector3d(axis);
    t.normalize();
    t.scale(radius - height);
    t.add(center);
    trans.setTranslation(t);
    double r, theta, h, s;
    int idx = 0;
    for (int i = 0; i < nR; i++) {
        for (int j = 0; j < nTheta; j++) {
            for (int k = 0; k < nH; k++) {
                h = k * dh + dh / 2;
                theta = j * dtheta + (i + k) * dtheta / 2;
                r = i * dr + dr / 2;
                s = Math.sqrt(-h * h + 2 * h * height - 2 * h * radius - height * height + 2 * height * radius) / radius;
                pnts[idx] = new CubaturePoint3d();
                pnts[idx].x = r * s * Math.cos(theta);
                pnts[idx].y = r * s * Math.sin(theta);
                pnts[idx].z = h;
                pnts[idx].w = sphereCapPartialVolume(height, radius, r, dr, h, dh, theta, dtheta);
                pnts[idx].transform(trans);
                idx++;
            }
        }
    }
    return pnts;
}

Example 83 with RigidTransform3d

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

the class FemBeamMech method build.

public void build(String[] args) {
    femPath = "models/mech/models/fem/";
    modPath = "models/mech/";
    int nn = 2;
    myFemMod = FemFactory.createTetGrid(null, 0.6, 0.2, 0.2, nn * 3, nn * 1, nn * 1);
    myFemMod.setName("fem");
    myFemMod.setDensity(myDensity);
    myFemMod.setBounds(new Point3d(-0.6, 0, 0), new Point3d(0.6, 0, 0));
    myFemMod.setLinearMaterial(60000, 0.33, true);
    myFemMod.setStiffnessDamping(0.002);
    myFemMod.setImplicitIterations(100);
    myFemMod.setImplicitPrecision(0.001);
    myFemMod.setSurfaceRendering(SurfaceRender.Shaded);
    Renderable elems = myFemMod.getElements();
    RenderProps.setLineWidth(elems, 2);
    RenderProps.setLineColor(elems, Color.BLUE);
    Renderable nodes = myFemMod.getNodes();
    RenderProps.setPointStyle(nodes, Renderer.PointStyle.SPHERE);
    RenderProps.setPointRadius(nodes, 0.005);
    RenderProps.setPointColor(nodes, Color.GREEN);
    // fix the leftmost nodes
    double EPS = 1e-9;
    for (FemNode3d n : myFemMod.getNodes()) {
        if (n.getPosition().x < -0.3 + EPS) {
            myLeftNodes.add(n);
        }
    }
    System.out.println("fixed nodes:");
    for (FemNode3d n : myLeftNodes) {
        n.setDynamic(false);
    }
    RenderProps.setFaceColor(myFemMod, new Color(0.4f, 0.4f, 1.0f));
    myFemMod.setProfiling(true);
    RigidBody anchorBox = new RigidBody("anchorBox");
    PolygonalMesh mesh = MeshFactory.createBox(0.1, 0.3, 0.3);
    anchorBox.setMesh(mesh, /* fileName= */
    null);
    RigidTransform3d X = new RigidTransform3d();
    X.p.set(-0.35, 0, 0);
    anchorBox.setPose(X);
    anchorBox.setDynamic(false);
    myMechMod = new MechModel("mech");
    myMechMod.addModel(myFemMod);
    myMechMod.addRigidBody(anchorBox);
    System.out.println("models: " + myMechMod.findComponent("models"));
    System.out.println("models/fem: " + myMechMod.findComponent("models/fem"));
    myMechMod.setIntegrator(Integrator.BackwardEuler);
    addModel(myMechMod);
    myMechMod.setProfiling(true);
    // add marker to lower right corner element
    Point3d corner = new Point3d(0.3, -0.1, -0.1);
    FemElement cornerElem = null;
    for (FemElement e : myFemMod.getElements()) {
        FemNode[] nodeList = e.getNodes();
        for (int i = 0; i < nodeList.length; i++) {
            if (nodeList[i].getPosition().epsilonEquals(corner, 1e-8)) {
                cornerElem = e;
                break;
            }
        }
    }
    if (cornerElem != null) {
        FemMarker mkr = new FemMarker(0.3, -0.07, -0.03);
        myFemMod.addMarker(mkr, cornerElem);
        RenderProps.setPointStyle(mkr, Renderer.PointStyle.SPHERE);
        RenderProps.setPointRadius(mkr, 0.01);
        RenderProps.setPointColor(mkr, Color.WHITE);
        Particle part = new Particle(1, 0.5, -0.07, -0.03);
        RenderProps.setPointStyle(part, Renderer.PointStyle.SPHERE);
        RenderProps.setPointRadius(part, 0.01);
        part.setDynamic(false);
        myMechMod.addParticle(part);
        AxialSpring spr = new AxialSpring(1000, 0, 0);
        myMechMod.attachAxialSpring(part, mkr, spr);
        RenderProps.setLineStyle(spr, Renderer.LineStyle.SPINDLE);
        RenderProps.setLineRadius(spr, 0.01);
        RenderProps.setLineColor(spr, Color.GREEN);
    }
    int numWays = 0;
    double res = 0.2;
    for (int i = 0; i < numWays; i++) {
        addWayPoint(new WayPoint((i + 1) * res, true));
    }
    addControlPanel(myMechMod, myFemMod);
}

Example 84 with RigidTransform3d

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

the class FemCollision method build.

public void build(String[] args) throws IOException {
    try {
        MechModel mechmod = new MechModel();
        // mechmod.setIntegrator (Integrator.ConstrainedBackwardEuler);
        mechmod.setIntegrator(Integrator.BackwardEuler);
        // mechmod.setProfiling (true);
        CollisionManager collisions = mechmod.getCollisionManager();
        RenderProps.setVisible(collisions, true);
        RenderProps.setEdgeWidth(collisions, 4);
        RenderProps.setEdgeColor(collisions, Color.YELLOW);
        RenderProps.setLineWidth(collisions, 3);
        RenderProps.setLineColor(collisions, Color.GREEN);
        collisions.setDrawContactNormals(true);
        RigidBody table = new RigidBody("table");
        table.setDynamic(false);
        // table.setMesh (new PolygonalMesh (new File (rbpath+ "box.obj")), null);
        table.setMesh(MeshFactory.createBox(2, 2, 2));
        AffineTransform3d trans = new AffineTransform3d();
        trans.setIdentity();
        trans.applyScaling(4, 2, 0.5);
        table.transformGeometry(trans);
        table.setPose(new RigidTransform3d(new Vector3d(1, 0, 0.8077474533228615), new AxisAngle(1, 0, 0, Math.toRadians(mu == 0 ? 0.0 : 1.5))));
        if (wireFrame) {
            RenderProps.setFaceStyle(table, Renderer.FaceStyle.NONE);
            RenderProps.setDrawEdges(table, true);
        }
        mechmod.addRigidBody(table);
        if (incBox0) {
            box0 = new RigidBody("box0");
            // box0.setMesh (
            // new PolygonalMesh (new File (rbpath + "box.obj")), null);
            box0.setMesh(MeshFactory.createBox(2, 2, 2));
            trans.setIdentity();
            trans.applyScaling(1.5, 1.5, 0.5);
            box0.transformGeometry(trans);
            box0.setInertia(SpatialInertia.createBoxInertia(10000, 4, 4, 1));
            box0.setPose(new RigidTransform3d(new Vector3d(-0.5, 0, 3.5), new AxisAngle()));
            RenderProps.setFaceColor(box0, Color.GREEN.darker());
            if (wireFrame) {
                RenderProps.setFaceStyle(box0, Renderer.FaceStyle.NONE);
                RenderProps.setDrawEdges(box0, true);
            }
            mechmod.addRigidBody(box0);
        }
        if (incFem0) {
            String fem0Name = // "torus546";
            "sphere2";
            // "box0023";
            // "box0048";
            // "box0144";
            // "box0604";
            // "box1056";
            // "box2463";
            // "box4257";
            fem0 = TetGenReader.read("fem0", 5000, fempath + fem0Name + ".1.node", fempath + fem0Name + ".1.ele", new Vector3d(0.8, 0.8, 0.8));
            fem0.transformGeometry(new RigidTransform3d(new Vector3d(2, 0, 3.5), new AxisAngle()));
            fem0.setLinearMaterial(1000000, 0.33, true);
            if (!wireFrame)
                fem0.setSurfaceRendering(SurfaceRender.Shaded);
            fem0.setParticleDamping(0.1);
            RenderProps.setFaceColor(fem0, new Color(0.5f, 0f, 0f));
            // RenderProps.setAlpha(fem0, 0.33);
            RenderProps.setAlpha(fem0, 0.5);
            RenderProps.setShading(fem0, Shading.NONE);
            RenderProps.setDrawEdges(fem0, true);
            RenderProps.setVisible(fem0.getElements(), false);
            RenderProps.setVisible(fem0.getNodes(), false);
            // RenderProps.setLineColor(fem0, Color.GRAY);
            mechmod.addModel(fem0);
        }
        if (incFem1) {
            // Use this code for a box
            /*
             * double mySize = 1; fem1 = createFem (name, mySize);
             * FemFactory.createTetGrid ( fem1, 1*mySize, 4*mySize, mySize, 1,
             * 1, 1);
             */
            // end box code
            // Use this code for a ball
            String fem1Name = "sphere2";
            fem1 = TetGenReader.read("fem1", 5000, fempath + fem1Name + ".1.node", fempath + fem1Name + ".1.ele", new Vector3d(0.8, 0.8, 0.8));
            fem1.setLinearMaterial(1000000, 0.33, true);
            fem1.setParticleDamping(0.1);
            // end ball code
            fem1.setIncompressible(FemModel3d.IncompMethod.AUTO);
            fem1.transformGeometry(new RigidTransform3d(new Vector3d(1.25, 0, 2), new AxisAngle()));
            fem1.setSurfaceRendering(wireFrame ? SurfaceRender.None : SurfaceRender.Shaded);
            RenderProps.setAlpha(fem1, 0.5);
            RenderProps.setShading(fem1, Shading.NONE);
            RenderProps.setDrawEdges(fem1, true);
            RenderProps.setVisible(fem1.getElements(), false);
            RenderProps.setVisible(fem1.getNodes(), false);
            RenderProps.setFaceColor(fem1, new Color(0f, 0f, 0.8f));
            mechmod.addModel(fem1);
        }
        if (incFem0) {
            mechmod.setCollisionBehavior(fem0, table, true, mu);
            mechmod.setCollisionResponse(fem0, Collidable.Deformable);
            mechmod.setCollisionResponse(fem0, table);
        }
        if (incFem1 & incFem0) {
            mechmod.setCollisionBehavior(fem0, fem1, true, mu);
        }
        if (incBox0 & incFem0) {
            mechmod.setCollisionBehavior(box0, fem0, true, mu);
        }
        if (incFem1) {
            mechmod.setCollisionBehavior(fem1, table, true, mu);
            mechmod.setCollisionResponse(fem1, Collidable.AllBodies);
        }
        if (incBox0 & incFem1) {
            mechmod.setCollisionBehavior(box0, fem1, true, mu);
        }
        if (incBox0) {
            mechmod.setCollisionBehavior(box0, table, true, mu);
            mechmod.setCollisionResponse(box0, table);
        }
        addModel(mechmod);
    // mechmod.setIntegrator (Integrator.BackwardEuler);
    // addBreakPoint (2.36);
    // reset();
    } catch (IOException e) {
        throw e;
    }
}

Example 85 with RigidTransform3d

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

the class FemCollision method reset.

void reset() {
    double r1 = rand.nextDouble();
    double r2 = rand.nextDouble();
    double r3 = rand.nextDouble();
    // case001: the blue ball has a very strange wobbling experience partly
    // inside the table, just after touching the red ball.
    r1 = 0.9339890095435555;
    r2 = 0.256644068300397;
    r3 = 0.005600826760283728;
    positionBall(fem0, new Vector3d(0.1, 0.1, 10 + r3 * 2));
    positionBall(fem1, testEdgeEdge ? new Vector3d(0, 0, 6) : new Vector3d(0, 0, 2));
    if (box0 != null) {
        box0.setPose(new RigidTransform3d(new Vector3d(-0.5, r1 * 0.2, 3.0 + r2), new AxisAngle()));
        box0.setVelocity(0, 0, 0, 0, 0, 0);
    }
    System.out.println("r1=" + r1 + "; r2=" + r2 + "; r3=" + r3 + ";");
}