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

Example 26 with Vector2d

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

the class MeshIntersectingProbe method findNextEdge.

// NOTE: only works for convex faces (so, triangular is okay)
// finds the next edge while clipping around in a circle
private static HalfEdge findNextEdge(LinkedList<Point3d> contour, ArrayList<Face> faceList, Intersector2d ti, SplitStorage info, Vector3d vx, Vector3d vy, Point3d o) {
    Vector3d dir = new Vector3d();
    Point3d vtxp = info.vtx.getWorldPoint();
    Point2d vtx2d = Intersector2d.get2dCoordinate(vtxp, vx, vy, o);
    dir.sub(contour.get(info.idx + 1), contour.get(info.idx));
    for (Face face : faceList) {
        HalfEdge he0 = face.getEdge(0);
        HalfEdge he = he0;
        do {
            if (info.vtx != he.head && info.vtx != he.tail) {
                Point2d p1 = Intersector2d.get2dCoordinate(he.head.getWorldPoint(), vx, vy, o);
                Point2d p2 = Intersector2d.get2dCoordinate(he.tail.getWorldPoint(), vx, vy, o);
                ArrayList<Point2d> pnts = new ArrayList<Point2d>();
                Vector2d lineDir = Intersector2d.get2dVector(dir, vx, vy);
                int npoints = ti.intersectLineLineSegment(vtx2d, lineDir, p1, p2, pnts);
                if (npoints == 1) {
                    Point3d p = Intersector2d.get3dCoordinate(pnts.get(0), vx, vy, o);
                    Vector3d ldir = new Vector3d(p.x - vtxp.x, p.y - vtxp.y, p.z - vtxp.z);
                    // check direction
                    if (ldir.dot(dir) > -ti.epsilon) {
                        // check if we passed the next point
                        Point3d pNext = contour.get(info.idx + 1);
                        if (ldir.norm() < pNext.distance(vtxp) + ti.epsilon) {
                            if (p.distance(pNext) < ti.epsilon) {
                                // move to next point
                                info.idx++;
                            }
                            info.vtx = createOrGetVertex(p, face.getVertices(), ti.epsilon);
                            return he;
                        } else {
                            // advance to next vertex
                            info.vtx = createOrGetVertex(pNext, face.getVertices(), ti.epsilon);
                            info.face = face;
                            // move to next point
                            info.idx++;
                            return null;
                        }
                    }
                }
            }
            he = he.getNext();
        } while (he != he0);
    }
    info.face = null;
    return null;
}

Example 27 with Vector2d

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

the class SurfaceMeshCollider method collideVerticesWithFaces.

/*
    * For each penetrating vertex, find the closest opposing face and add a
    * corresponding new element to the list of ContactPenetratingPoints
    */
public static void collideVerticesWithFaces(ArrayList<PenetratingPoint> cpps, PenetrationRegion region, PolygonalMesh otherMesh) {
    BVFeatureQuery query = new BVFeatureQuery();
    Vector2d uv = new Vector2d();
    Point3d nearPnt = new Point3d();
    Vector3d disp = new Vector3d();
    Point3d wpnt = new Point3d();
    PenetratingPoint cpp;
    for (Vertex3d vtx : region.myVertices) {
        vtx.getWorldPoint(wpnt);
        Face face = query.nearestFaceToPoint(nearPnt, uv, otherMesh, wpnt);
        disp.sub(nearPnt, wpnt);
        cpp = new PenetratingPoint(vtx, face, uv, nearPnt, disp, region);
        cpps.add(cpp);
    }
}

Example 28 with Vector2d

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

the class NURBSCurve2dTest method addTestCurves.

public void addTestCurves() {
    Vector4d[] q4 = new Vector4d[] { new Vector4d(300, 100, 0, 1), new Vector4d(300, 300, 0, 1), new Vector4d(100, 300, 0, 1), new Vector4d(100, 100, 0, 1) };
    double[] k4 = new double[] { -2, -1, 0, 1, 2, 3, 4, 5, 6 };
    Vector4d[] qclosed = new Vector4d[] { new Vector4d(150, 50, 0, 1), new Vector4d(250, 50, 0, 1), new Vector4d(300, 100, 0, 1), new Vector4d(350, 150, 0, 1), new Vector4d(350, 250, 0, 1), new Vector4d(300, 300, 0, 1), new Vector4d(250, 350, 0, 1), new Vector4d(150, 350, 0, 1), new Vector4d(100, 300, 0, 1), new Vector4d(50, 250, 0, 1), new Vector4d(50, 150, 0, 1), new Vector4d(100, 100, 0, 1) };
    double[] kclosed = new double[] { 0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5 };
    Vector4d[] qopen = new Vector4d[] { new Vector4d(100, 100, 0, 1), new Vector4d(150, 50, 0, 1), new Vector4d(250, 50, 0, 1), new Vector4d(300, 100, 0, 1), new Vector4d(350, 150, 0, 1), new Vector4d(350, 250, 0, 1), new Vector4d(300, 300, 0, 1), new Vector4d(250, 350, 0, 1), new Vector4d(150, 350, 0, 1), new Vector4d(100, 300, 0, 1), new Vector4d(50, 250, 0, 1), new Vector4d(50, 150, 0, 1), new Vector4d(100, 100, 0, 1) };
    double[] kopen = new double[] { 1, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 11 };
    Vector4d[] qsimple = new Vector4d[] { new Vector4d(100, 100, 0, 1), new Vector4d(200, 100, 0, 1), new Vector4d(200, 200, 0, 1), new Vector4d(200, 300, 0, 1), new Vector4d(100, 300, 0, 1), new Vector4d(50, 200, 0, 1) };
    double[] ksimple = new double[] { 0, 0, 1, 1 };
    // NURBSCurve2d curve = new NURBSCurve2d (2, NURBSCurve2d.CLOSED, q4, null);
    NURBSCurve2d curve = new NURBSCurve2d(2, NURBSCurve2d.CLOSED, qsimple, null);
    curve.convertToBezier();
    writeCurve("curve.txt", curve);
    NURBSCurve2d curve2 = readCurve("curve.txt");
    NURBSCurve2d check = new NURBSCurve2d(3, NURBSCurve2d.CLOSED, q4, null);
    check.transform(new RigidTransform3d(300, 0, 0));
    DistanceGrid grid = new DistanceGrid(150, 200, 500, 500, 51, 51);
    grid.update(curve2, 50);
    QuadBezierDistance2d dist = new QuadBezierDistance2d(curve2);
    Vector2d pnt = new Vector2d(250, 200);
    Vector2d near = new Vector2d();
    double d = dist.computeDistance(near, pnt, 1e10);
    System.out.println("max curvature=" + dist.computeMaxCurvature());
    myFrame.addRenderable(curve2);
    myFrame.addRenderable(grid);
}

Example 29 with Vector2d

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

the class ContactInfo method computePenetratingPoints.

ArrayList<PenetratingPoint> computePenetratingPoints(PolygonalMesh mesh0, PolygonalMesh mesh1) {
    BVFeatureQuery query = new BVFeatureQuery();
    Point3d wpnt = new Point3d();
    Point3d nearest = new Point3d();
    Vector2d uv = new Vector2d();
    Vector3d disp = new Vector3d();
    ArrayList<PenetratingPoint> points = new ArrayList<PenetratingPoint>();
    for (Vertex3d vtx : mesh0.getVertices()) {
        // John Lloyd, Jan 3, 2014: rewrote to use isInsideOrientedMesh()
        // to determine if a vertex is inside another mesh. Previous code
        // would not always work and broke when the BVTree code was
        // refactored.
        vtx.getWorldPoint(wpnt);
        if (query.isInsideOrientedMesh(mesh1, wpnt, -1)) {
            Face f = query.getFaceForInsideOrientedTest(nearest, uv);
            mesh1.transformToWorld(nearest);
            disp.sub(nearest, wpnt);
            points.add(new PenetratingPoint(vtx, f, uv, nearest, disp, /*region=*/
            null));
        }
    }
    return points;
}

Example 30 with Vector2d

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

the class MFreeMeshComp method createPointAttachment.

public PointFem3dAttachment createPointAttachment(Point pnt) {
    if (!(getMesh() instanceof PolygonalMesh)) {
        return null;
    }
    PolygonalMesh mesh = (PolygonalMesh) getMesh();
    if (!mesh.isTriangular()) {
        return null;
    }
    // Find nearest face to the point. The vertices of this face will be used
    // to find the nodes and weight for the attachment.
    BVFeatureQuery query = new BVFeatureQuery();
    Point3d near = new Point3d();
    Vector2d uv = new Vector2d();
    Face face = query.nearestFaceToPoint(near, uv, mesh, pnt.getPosition());
    Vertex3d[] vtxs = face.getTriVertices();
    double[] wgts = new double[] { 1 - uv.x - uv.y, uv.x, uv.y };
    HashMap<FemNode, Double> nodeWeights = new HashMap<FemNode, Double>();
    for (int i = 0; i < vtxs.length; i++) {
        PointAttachment va = myVertexAttachments.get(vtxs[i].getIndex());
        if (va instanceof PointParticleAttachment) {
            PointParticleAttachment ppa = (PointParticleAttachment) va;
            FemNode node = (FemNode) ppa.getParticle();
            accumulateNodeWeights(node, wgts[i], nodeWeights);
        } else if (va instanceof PointFem3dAttachment) {
            PointFem3dAttachment pfa = (PointFem3dAttachment) va;
            for (int k = 0; k < pfa.numMasters(); k++) {
                FemNode node = pfa.getNodes()[k];
                double w = pfa.getCoordinate(k);
                accumulateNodeWeights(node, w * wgts[i], nodeWeights);
            }
        }
    }
    // Create a new PointFem3dAttachment
    PointFem3dAttachment ax = new PointFem3dAttachment(pnt);
    VectorNd weightVec = new VectorNd();
    for (Double d : nodeWeights.values()) {
        weightVec.append(d);
    }
    ax.setFromNodes(nodeWeights.keySet(), weightVec);
    return ax;
}