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

Example 31 with Vector2d

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

the class DistanceGridSurfCalc method findSurfaceTangentInTet.

protected boolean findSurfaceTangentInTet(Point3d ptLoc, TetDesc tdesc, Point3d paLoc, Plane planeLoc) {
    // XXX convert normal, off, and pa to tet coordinates
    Plane planeCell = new Plane();
    Point3d paCell = new Point3d();
    transformToQuadCell(planeCell, planeLoc, tdesc);
    transformToQuadCell(paCell, paLoc, tdesc);
    double[] c = new double[10];
    double[] b = new double[6];
    myGrid.computeQuadCoefs(c, tdesc);
    PlaneType planeType;
    Vector2d[] pnts = new Vector2d[] { new Vector2d(), new Vector2d() };
    Vector3d r = new Vector3d();
    planeType = computeBCoefs(b, r, c, planeCell);
    double x = 0, y = 0, z = 0;
    boolean found = false;
    switch(planeType) {
        case YZ:
            {
                int nr = findTangentPoints(pnts, b, paCell.y, paCell.z);
                for (int i = 0; i < nr; i++) {
                    y = pnts[i].x;
                    z = pnts[i].y;
                    x = r.z - r.x * y - r.y * z;
                    if (tdesc.myTetId.isInside(x, y, z)) {
                        found = true;
                        break;
                    }
                }
                break;
            }
        case ZX:
            {
                int nr = findTangentPoints(pnts, b, paCell.z, paCell.x);
                for (int i = 0; i < nr; i++) {
                    z = pnts[i].x;
                    x = pnts[i].y;
                    y = r.z - r.x * z - r.y * x;
                    if (tdesc.myTetId.isInside(x, y, z)) {
                        found = true;
                        break;
                    }
                }
                break;
            }
        case XY:
            {
                int nr = findTangentPoints(pnts, b, paCell.x, paCell.y);
                for (int i = 0; i < nr; i++) {
                    x = pnts[i].x;
                    y = pnts[i].y;
                    z = r.z - r.x * x - r.y * y;
                    if (tdesc.myTetId.isInside(x, y, z)) {
                        found = true;
                        break;
                    }
                }
                break;
            }
    }
    if (found) {
        // System.out.println ("dist=" + computeDist (c, x, y, z));
        // System.out.println ("perp=" + computePerp (c, x, y, z, paCell));
        transformFromQuadCell(ptLoc, x, y, z, tdesc);
        return true;
    } else {
        return false;
    }
}

Example 32 with Vector2d

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

the class DistanceGridSurfCalc method computePlanePerp.

// for debugging
private double computePlanePerp(double[] b, double x, double y, double px, double py) {
    Vector2d grad = new Vector2d();
    grad.x = 2 * b[0] * x + b[2] * y + b[3];
    grad.y = 2 * b[1] * y + b[2] * x + b[4];
    Vector2d diff = new Vector2d(x, y);
    diff.x -= px;
    diff.y -= py;
    return diff.dot(grad) / (grad.norm() * diff.norm());
}

Example 33 with Vector2d

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

the class DistanceGridFeatureQuery method isInsideOrientedMesh.

/**
 * Returns true if a point is on or inside an oriented triangular mesh, the
 * faces of which are contained within a specified distance grid.
 * "Oriented" means that all face normals are assumed to point
 * outwards.
 *
 * <p> The method works by inspecting the nearest face, edge or vertex to
 * the point. Hence the mesh does not need to be closed, and the method is
 * faster, though possibly less numerically robust, than {@link
 * #isInsideMesh(PolygonalMesh,DistanceGrid,Point3d,double)}.
 *
 * @param grid distance grid containing the faces.
 * @param pnt point to check (in world coordinates)
 * @param tol tolerance within which the point is considered to be on the
 * mesh surface. A value of -1 will cause the tolerance to be computed
 * automatically.
 * @return true if <code>pnt</code> is on or inside the mesh.
 */
public boolean isInsideOrientedMesh(DistanceGrid grid, Point3d pnt, double tol) {
    Point3d lpnt;
    if (grid.getLocalToWorld() != null && grid.getLocalToWorld() != RigidTransform3d.IDENTITY) {
        lpnt = new Point3d(pnt);
        lpnt.inverseTransform(grid.getLocalToWorld());
    } else {
        lpnt = pnt;
    }
    if (tol < 0) {
        tol = 1e-12 * grid.getRadius();
    }
    lastFace = null;
    lastNear = null;
    lastUV = null;
    double d = grid.getLocalDistance(lpnt);
    if (d == DistanceGrid.OUTSIDE_GRID) {
        lastCase = "Culled";
        return false;
    }
    Point3d nearest = new Point3d();
    Feature f = grid.getNearestLocalFeature(nearest, lpnt);
    if (f == null || !(f instanceof Face)) {
        // must be no faces in the mesh
        lastCase = "No Faces";
        return false;
    }
    Face face = (Face) f;
    Vector3d diff = new Vector3d();
    Vector2d uv = new Vector2d();
    face.computeCoords(nearest, uv);
    // diff is the vector from the nearest face point to pnt, in face coords
    diff.sub(lpnt, nearest);
    if (diff.norm() <= tol) {
        lastCase = "Tol";
        return true;
    }
    // w0, w1, w2 are the barycentric weights of the intersection point
    // with respect to the face vertices
    double w0 = 1 - uv.x - uv.y;
    double w1 = uv.x;
    double w2 = uv.y;
    // tolerence to determine if the intersection point is near an edge or
    // vertex
    double eps = 1e-12;
    Vertex3d nearVertex = null;
    HalfEdge nearEdge = null;
    if (Math.abs(w0 - 1) < eps) {
        // intersection is near vertex 0
        nearVertex = face.getVertex(0);
    } else if (Math.abs(w1 - 1) < eps) {
        // intersection is near vertex 1
        nearVertex = face.getVertex(1);
    } else if (Math.abs(w2 - 1) < eps) {
        // intersection is near vertex 2
        nearVertex = face.getVertex(2);
    } else if (Math.abs(w0) < eps) {
        // intersection is near edge 1-2
        nearEdge = face.getEdge(2);
    } else if (Math.abs(w1) < eps) {
        // intersection is near edge 0-2
        nearEdge = face.getEdge(0);
    } else if (Math.abs(w2) < eps) {
        // intersection is near edge 0-1
        nearEdge = face.getEdge(1);
    }
    boolean inside;
    if (nearVertex != null) {
        numVertexCases++;
        Iterator<HalfEdge> it = nearVertex.getIncidentHalfEdges();
        HalfEdge he = it.next();
        double dot = diff.dot(he.getFace().getNormal());
        int uniformSign = SGN(dot);
        while (uniformSign != 0 && it.hasNext()) {
            he = it.next();
            dot = diff.dot(he.getFace().getNormal());
            if (uniformSign != SGN(dot)) {
                uniformSign = 0;
            }
        }
        if (uniformSign != 0) {
            lastCase = "Vertex uniform sign check";
            inside = (uniformSign < 0);
        } else {
            lastCase = "Vertex outward pointing check";
            inside = !vertexPointsOutward(nearVertex);
        }
    } else if (nearEdge != null && nearEdge.opposite != null) {
        numEdgeCases++;
        Face oppface = nearEdge.opposite.getFace();
        double dot0 = diff.dot(face.getNormal());
        double dot1 = diff.dot(oppface.getNormal());
        if (Math.abs(dot0) > Math.abs(dot1)) {
            inside = dot0 < 0;
        } else {
            inside = dot1 < 0;
        }
        lastCase = "Edge";
    } else {
        numFaceCases++;
        inside = diff.dot(face.getNormal()) < 0;
        lastCase = "Face";
    }
    // save in case of query in getFaceForInsideOrientedTest
    lastFace = face;
    lastNear = new Point3d(nearest);
    lastUV = uv;
    return inside;
}

Example 34 with Vector2d

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

the class Intersector2d method intersectLineSegmentLineSegment.

public int intersectLineSegmentLineSegment(Point2d p1a, Point2d p1b, Point2d p2a, Point2d p2b, ArrayList<Point2d> points) {
    Vector2d dir1 = new Vector2d(p1b);
    dir1.sub(p1a);
    double l1 = dir1.norm();
    // normalize
    dir1.scale(1.0 / l1);
    Vector2d dir2 = new Vector2d(p2b);
    dir2.sub(p2a);
    double l2 = dir2.norm();
    dir2.scale(1.0 / l2);
    // circle test
    Point2d a = new Point2d();
    a.combine(0.5, p1a, 0.5, p1b);
    Point2d b = new Point2d();
    b.combine(0.5, p2a, 0.5, p2b);
    // check if within each other's radii
    if (a.distance(b) > (l1 + l2) / 2 + epsilon) {
        return 0;
    }
    // within range, so intersect lines
    ArrayList<Point2d> tmpPnts = new ArrayList<Point2d>();
    int nAdded = intersectLineLine(p1a, dir1, p2a, dir2, tmpPnts);
    // we already know it lies on both lines, so just check distances
    if (nAdded == 1) {
        Point2d p = tmpPnts.get(0);
        if (p.distance(p1a) + p.distance(p1b) > l1 + epsilon) {
            return 0;
        }
        if (p.distance(p2a) + p.distance(p2b) > l2 + epsilon) {
            return 0;
        }
        points.add(p);
        return 1;
    }
    // parallel
    if (nAdded == 0) {
        return 0;
    }
    // colinear
    Vector2d v = dir1;
    // get direction vector
    double tmp;
    // end points of interval
    // segment 1: [t1s, t1e]
    double t1s = p1a.dot(v);
    double t1e = p1b.dot(v);
    if (t1s < t1e) {
        tmp = t1s;
        t1s = t1e;
        t1e = tmp;
    }
    // segment 2: [t2s, t2e]
    double t2s = p2a.dot(v);
    double t2e = p2b.dot(v);
    if (t2s < t2e) {
        tmp = t2s;
        t2s = t2e;
        t2e = tmp;
    }
    double ts = Math.max(t1s, t2s);
    double te = Math.min(t1e, t2e);
    if (te > ts - epsilon) {
        Vector2d n = new Vector2d();
        n.scaledAdd(-p1a.dot(v), v, p1a);
        a.scaledAdd(ts, v, n);
        b.scaledAdd(te, v, n);
        if (a.distance(b) < epsilon) {
            points.add(a);
            return 1;
        } else {
            points.add(a);
            points.add(b);
            return 2;
        }
    }
    return nAdded;
}

Example 35 with Vector2d

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

the class Intersector2d method intersectLineLineSegment.

public int intersectLineLineSegment(Point2d p1, Vector2d v1, Point2d p2a, Point2d p2b, ArrayList<Point2d> points) {
    Vector2d v2 = new Vector2d(p2b.x - p2a.x, p2b.y - p2a.y);
    // intersect lines
    ArrayList<Point2d> tmpPnts = new ArrayList<Point2d>();
    int nAdded = intersectLineLine(p1, v1, p2a, v2, tmpPnts);
    if (nAdded == 1) {
        Point2d p = tmpPnts.get(0);
        if (p.distance(p2a) + p.distance(p2b) > p2a.distance(p2b) + epsilon) {
            return 0;
        }
        points.add(p);
        return 1;
    }
    if (nAdded == 2) {
        points.addAll(tmpPnts);
        return 2;
    }
    return 0;
}