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

Example 11 with Vector3d

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

the class ContactPlane method build.

// public boolean build (MeshIntersectionContour aContour, double pointTol) {
// if (aContour.isClosed && aContour.size() > 1) {
// contour = aContour;
// pointTolerance = pointTol;
// PenetrationRegion region0 = new PenetrationRegion();
// PenetrationRegion region1 = new PenetrationRegion();
// region0.setInsideVertices (aContour.insideVertices0);
// region1.setInsideVertices (aContour.insideVertices1);
// region0.setInsideFaces (aContour.insideFaces0);
// region1.setInsideFaces (aContour.insideFaces1);
// return compute(region0, region1);
// }
// else {
// return false;
// }
// }
// 
public boolean build(PenetrationRegion region0, PenetrationRegion region1, PolygonalMesh mesh0, double pointTol) {
    contour = null;
    if (region0.myContours.size() == 1) {
        IntersectionContour c = region0.getFirstContour();
        if (c.isClosed && c.size() > 1) {
            contour = region0.getFirstContour();
        }
    } else {
        Vector3d areaVec = new Vector3d();
        Vector3d centroid = new Vector3d();
        double maxa = 0;
        IntersectionContour maxc = null;
        for (IntersectionContour c : region0.myContours) {
            if (c.isClosed && c.size() >= 3) {
                c.fitPlane(areaVec, centroid, pointTol);
                double a = areaVec.norm();
                if (a > maxa) {
                    maxc = c;
                    maxa = a;
                }
            }
        }
        if (maxc == null) {
            for (IntersectionContour c : region0.myContours) {
                if (c.isClosed && c.size() > 1) {
                    maxc = c;
                    break;
                }
            }
        }
        if (maxc != null) {
            contour = maxc;
        }
    }
    pointTolerance = pointTol;
    if (contour != null) {
        return compute(region0, region1, mesh0);
    } else {
        return false;
    }
}

Example 12 with Vector3d

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

the class ContactPlane method edgeEdgeContact.

/*
    * Handle the case where there are insufficient contact points in the contour
    * to define a plane. and there are no penetrating vertices in either region.
    */
boolean edgeEdgeContact(PenetrationRegion region0, PenetrationRegion region1, PolygonalMesh mesh0) {
    HashSet<HalfEdge> edges = new HashSet<HalfEdge>();
    for (IntersectionPoint mip : contour) edges.add(mip.edge);
    if (edges.size() != 2) {
        /*
          * This can happen if a penetrating vertex goes right through the other
          * object and out the other side, or if an edge is co-planar with a
          * face.
          */
        System.out.println("unknown contact type: edge-edge with " + edges.size() + " edges");
        // with "+edges.size()+" edges");
        return false;
    }
    HalfEdge edge0, edge1;
    Iterator<HalfEdge> itr = edges.iterator();
    edge0 = itr.next();
    edge1 = itr.next();
    Vector3d vedge0 = new Vector3d(), vedge1 = new Vector3d();
    vedge0.sub(edge0.getHead().getWorldPoint(), edge0.getTail().getWorldPoint());
    vedge1.sub(edge1.getHead().getWorldPoint(), edge1.getTail().getWorldPoint());
    normal.cross(vedge0, vedge1);
    normal.normalize();
    /* Compute depth=perpendicular distance between the two edges. */
    Vector3d c = new Vector3d();
    c.sub(edge1.getTail().getWorldPoint(), edge0.getTail().getWorldPoint());
    vedge0.cross(vedge0, vedge1);
    depth = Math.abs(c.dot(vedge0)) / vedge0.norm();
    minProjectedDistance = maxProjectedDistance = 0;
    checkNormalDirection(region0.getFaces(), region1.getFaces(), mesh0);
    return true;
}

Example 13 with Vector3d

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

the class ContactPlane method turn.

// public ContactPlane (
// PenetrationRegion region0, PenetrationRegion region1, double pointTol) {
// build (region0, region1, pointTol);
// }
// 
double turn(Point3d p0, Point3d p1, Point3d p2, Vector3d nrm) {
    Vector3d del01 = new Vector3d();
    Vector3d del12 = new Vector3d();
    Vector3d xprod = new Vector3d();
    del01.sub(p1, p0);
    del12.sub(p2, p1);
    xprod.cross(del01, del12);
    return xprod.dot(nrm);
}

Example 14 with Vector3d

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

the class ContactPlane method normalContact.

/*
    * Handle the case where there are enough contact points in the contour to
    * define a plane.
    */
boolean normalContact(PenetrationRegion region0, PenetrationRegion region1, PolygonalMesh mesh0) {
    /*
       * Project each point into the plane. Calculate the radius of the
       * projected point from the centroid. Also calculate the maximum distance
       * of a mesh intersection point from either side of the plane. This
       * maximum depth includes all points, even concave ones which may later be
       * removed from the contour.
       */
    Point3d proj = new Point3d();
    for (IntersectionPoint mip : mPoints) {
        proj.sub(mip, centroid);
        // signed distance from plane to the mip
        double s = proj.dot(normal);
        // along normal
        if (s < minProjectedDistance) {
            minProjectedDistance = s;
        }
        if (s > maxProjectedDistance) {
            maxProjectedDistance = s;
        }
        proj.scaledAdd(-s, normal);
    // mip.radius = proj.norm();
    }
    /*
       * Distance between two planes parallel to the fitted plane which contain
       * all the intersection points between them.
       */
    depth = maxProjectedDistance - minProjectedDistance;
    /*
       * Remove concave points to make the points a convex hull. A point p1 is
       * concave with respect to the preceding and following points p0 and p2
       * if the point sequence p0, p1, p2 forms a right turn with respect to
       * the normal.
       */
    boolean removedPoint;
    Vector3d xprod = new Vector3d();
    do {
        removedPoint = false;
        IntersectionPoint p0;
        IntersectionPoint p1 = mPoints.get(0);
        IntersectionPoint p2 = mPoints.get(1);
        int i = 2;
        for (int k = 1; k <= mPoints.size(); k++) {
            p0 = p1;
            p1 = p2;
            p2 = mPoints.get(i % mPoints.size());
            if (turn(p0, p1, p2, normal) < 0) {
                mPoints.remove((i - 1) % mPoints.size());
                removedPoint = true;
                p1 = p0;
            } else {
                i++;
            }
        }
    } while (removedPoint);
    /*
       * Adjust depth so it is greater than or equal to the maximum distance
       * from a vertex of either region to the opposing face of that vertex.
       * 
       * Also try to figure out which way the normal should point. It's
       * required by RigidBodyContact to point in the direction of the force to
       * be applied to mesh0 to stop it from penetrating mesh1. For
       * pathological contours this may be ambiguous, but in the simple case
       * where the contour fits well to a plane, the ContactRegion normal
       * should point from the penetrating vertices of mesh0 to the opposing
       * faces of mesh1.
       * 
       * If the contour is too confusing to distinguish a direction then throw
       * an error.
       */
    if (region0.numVertices() + region1.numVertices() == 0) {
        checkNormalDirection(region0.getFaces(), region1.getFaces(), mesh0);
    } else {
        double dTotal = 0;
        BVFeatureQuery query = new BVFeatureQuery();
        Point3d wpnt = new Point3d();
        Point3d nearest = new Point3d();
        Vector2d coords = new Vector2d();
        Vector3d diff = new Vector3d();
        for (Vertex3d v : region0.myVertices) {
            v.getWorldPoint(wpnt);
            if (query.isInsideOrientedMesh(region1.myMesh, wpnt, 0)) {
                query.getFaceForInsideOrientedTest(nearest, coords);
                region1.myMesh.transformToWorld(nearest);
                diff.sub(wpnt, nearest);
                double d = diff.dot(normal);
                dTotal += d;
                if (d < 0) {
                    d = -d;
                }
                if (d > depth) {
                    depth = d;
                }
            }
        }
        for (Vertex3d v : region1.myVertices) {
            v.getWorldPoint(wpnt);
            if (query.isInsideOrientedMesh(region0.myMesh, wpnt, 0)) {
                query.getFaceForInsideOrientedTest(nearest, coords);
                region0.myMesh.transformToWorld(nearest);
                diff.sub(wpnt, nearest);
                double d = diff.dot(normal);
                dTotal -= d;
                if (d < 0) {
                    d = -d;
                }
                if (d > depth) {
                    depth = d;
                }
            }
        }
        if (dTotal > 0) {
            negateNormal();
        }
    }
    return true;
}

Example 15 with Vector3d

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

the class SDGridTest method gridToLocal.

Vector3d gridToLocal(VectorTransformer3d TGL, double x, double y, double z) {
    Vector3d loc = new Vector3d();
    TGL.transformPnt(loc, new Vector3d(x, y, z));
    return loc;
}