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

Example 36 with Point3d

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

the class DistanceGridSurfCalc method transformToQuadCell.

public void transformToQuadCell(Plane pc, Plane pl, TetDesc tdesc) {
    Point3d p = new Point3d();
    // point on the untransformed plane
    p.scale(pl.offset, pl.normal);
    // normal transform to quad grid same as transform to regular grid:
    myGrid.myGridToLocal.inverseTransformCovec(pc.normal, pl.normal);
    pc.normal.normalize();
    // transform ref point to quad cell to recompute the offset
    transformToQuadCell(p, p, tdesc);
    pc.offset = p.dot(pc.normal);
}

Example 37 with Point3d

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

the class DistanceGridSurfCalc method findQuadSurfaceIntersectionLoc.

/**
 * Find the nearest quad surface intersection to p0, in the direction dir,
 * and return the result in ps. It is assumed that dir lies in the
 * plane. Input and output parameters are all given in grid local
 * coordinates. The method returns a descriptor of the tet/plane
 * intersection for the tet containing ps, unless ps is not found, in which
 * case null is returned.
 */
public TetDesc findQuadSurfaceIntersectionLoc(Point3d ps, Point3d p0, Vector3d dir) {
    TetDesc tdesc = null;
    // find the quadratic cell containing p0
    Vector3i vxyz = new Vector3i();
    Vector3d xyz = new Vector3d();
    if (myGrid.getQuadCellCoords(xyz, vxyz, p0, myGrid.myQuadGridToLocal) == -1) {
        // shouldn't happen - grid should have enough margin to prevent this
        if (myDebug > 0) {
            System.out.println("Not found: no quad cell found for p0");
        }
        return null;
    }
    // find the tet containing p0 within the quadratic cell
    tdesc = new TetDesc(vxyz, TetID.findSubTet(xyz.x, xyz.y, xyz.z));
    // now transform p0 and dir to quad grid coordinates
    Point3d p0Quad = new Point3d();
    Vector3d dirQuad = new Vector3d();
    myGrid.myQuadGridToLocal.inverseTransformPnt(p0Quad, p0);
    myGrid.myQuadGridToLocal.inverseTransformVec(dirQuad, dir);
    if (myDebug > 0) {
        System.out.println("looking for ps starting from " + tdesc);
    }
    // Find the interval (srng[0], srng[1]) within which the ray (p0, dir)
    // intersects tet/plane intersection defined by isect. This interval is
    // used to seed the surface intersection search. The edge of the
    // intersection boundary which clips the upper value of the range is
    // returned in edgeIdx, and the intersection parameter along that edge is
    // placed in edgeS.
    double[] srng = new double[] { 0, Double.MAX_VALUE };
    tdesc.clipLineSegment(srng, p0Quad, dirQuad);
    if (srng[0] > srng[1]) {
        // shouldn't happen - p0 should be in the tet
        if (myDebug > 0) {
            System.out.println("Not found: p0 tet does not intersect ray (p0,dir)");
        }
        return null;
    }
    // Find the tet boundary feature associated with the upper bound of the
    // ray intersection. This will be used to search for adjacent tets in
    // case the surface intersection does not occur within the current tet.
    Point3d px = new Point3d();
    px.scaledAdd(srng[1], dir, p0);
    // TetFeature lastFeat = isect.getFeature (edgeS.value, edgeIdx);
    TetFeature lastFeat = findNearestFeature(tdesc, px, 1e-10);
    myVisitedTets.clear();
    myVisitedTets.add(tdesc);
    // first point ps at which (p0, dir) intersects the quadratic surface.
    while (findSurfaceIntersectionInTet(ps, tdesc, srng, p0Quad, dirQuad) == CONTINUE) {
        // A return value of CONTINUE means that the surface intersection was
        // not found in the current tet, and that we should look for the
        // intersection in adjacent tets.
        ArrayList<TetDesc> adescs = new ArrayList<TetDesc>();
        int ntets = getFeatureAdjacentTets(adescs, tdesc, lastFeat, myVisitedTets);
        TetDesc tbest = null;
        double bestLen = 0;
        if (myDebug > 0) {
            System.out.println("checking " + ntets + " adjacent tets for feature " + lastFeat);
            System.out.print("  ");
            for (int k = 0; k < ntets; k++) {
                System.out.print(adescs.get(k) + " ");
            }
            System.out.println("");
        }
        // intersection intersects the ray over the longest length.
        for (int k = 0; k < ntets; k++) {
            TetDesc adesc = adescs.get(k);
            double[] irng = new double[] { 0, Double.MAX_VALUE };
            adesc.clipLineSegment(irng, p0Quad, dirQuad);
            double ilen = irng[1] - irng[0];
            if (myDebug > 0) {
                System.out.println("    ilen=" + ilen);
            }
            if (ilen > bestLen) {
                bestLen = ilen;
                tbest = adesc;
                srng[0] = irng[0];
                srng[1] = irng[1];
            }
        }
        if (tbest == null) {
            if (myDebug > 0) {
                System.out.println("Not found: no adjacent tets intersect the ray");
            }
            return null;
        } else {
            px.scaledAdd(srng[1], dir, p0);
            // lastFeat = ibest.getFeature (bestS, bestEdgeIdx);
            lastFeat = findNearestFeature(tbest, px, 1e-10);
            tdesc = tbest;
            myVisitedTets.add(tdesc);
        }
    }
    myGrid.myQuadGridToLocal.transformPnt(ps, ps);
    return tdesc;
}

Example 38 with Point3d

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

the class Face method isPointInside.

public boolean isPointInside(double x, double y, double z) {
    if (myNormal == null)
        // Make sure referenceArea is current.
        computeNormal();
    double tol = referenceArea * insideTriangleTolerance;
    Point3d p0 = he0.tail.getWorldPoint();
    double xp0 = x - p0.x;
    double yp0 = y - p0.y;
    double zp0 = z - p0.z;
    Point3d p1 = he0.head.getWorldPoint();
    double xp1 = x - p1.x;
    double yp1 = y - p1.y;
    double zp1 = z - p1.z;
    double xa = yp0 * zp1 - zp0 * yp1;
    double ya = zp0 * xp1 - xp0 * zp1;
    double za = xp0 * yp1 - yp0 * xp1;
    double q0 = Math.sqrt(xa * xa + ya * ya + za * za);
    double q = referenceArea - q0;
    if (q < -tol) {
        if (debugIntersect)
            System.out.println(" fail 1 q=" + q + " ra=" + referenceArea);
        return false;
    }
    Point3d p2 = he0.next.head.getWorldPoint();
    double xp2 = x - p2.x;
    double yp2 = y - p2.y;
    double zp2 = z - p2.z;
    xa = yp0 * zp2 - zp0 * yp2;
    ya = zp0 * xp2 - xp0 * zp2;
    za = xp0 * yp2 - yp0 * xp2;
    q0 = Math.sqrt(xa * xa + ya * ya + za * za);
    q = q - q0;
    if (q < -tol) {
        if (debugIntersect)
            System.out.println(" fail 2 q=" + q + " ra=" + referenceArea);
        return false;
    }
    xa = yp1 * zp2 - zp1 * yp2;
    ya = zp1 * xp2 - xp1 * zp2;
    za = xp1 * yp2 - yp1 * xp2;
    q0 = Math.sqrt(xa * xa + ya * ya + za * za);
    q = q - q0;
    if (q < 0) {
        // the point is inside.
        if (q < -tol) {
            if (debugIntersect)
                System.out.println(" fail 3 q=" + q + " ra=" + referenceArea);
            return false;
        }
    }
    return true;
}

Example 39 with Point3d

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

the class Face method computeCovariance.

/**
 * Computes covariance of this face and returns its area. This is done by
 * subdividing the face into a triangular fan centered on the first vertex,
 * and adding the covariance and area for all the triangles.
 *
 * @param C
 * returns the covariance
 * @return area of the face
 */
public double computeCovariance(Matrix3d C) {
    double area = 0;
    C.setZero();
    HalfEdge he = he0;
    Point3d p0 = he.head.pnt;
    he = he.next;
    Point3d p1 = he.head.pnt;
    he = he.next;
    Point3d p2 = he.head.pnt;
    do {
        double a = CovarianceUtils.addTriangleCovariance(C, p0, p1, p2);
        area += a;
        // // compute and add triangle area
        // double d1x = d2x;
        // double d1y = d2y;
        // double d1z = d2z;
        // d2x = p2.x - p0.x;
        // d2y = p2.y - p0.y;
        // d2z = p2.z - p0.z;
        // double nx = d1y * d2z - d1z * d2y;
        // double ny = d1z * d2x - d1x * d2z;
        // double nz = d1x * d2y - d1y * d2x;
        // double a = Math.sqrt (nx*nx + ny*ny + nz*nz)/2;
        // area += a;
        // // compute and add covariance for triangle
        // double pcx = (p0.x + p1.x + p2.x) / 3;
        // double pcy = (p0.y + p1.y + p2.y) / 3;
        // double pcz = (p0.z + p1.z + p2.z) / 3;
        // C.m00 += a * (9*pcx*pcx + p0.x*p0.x + p1.x*p1.x + p2.x*p2.x);
        // C.m11 += a * (9*pcy*pcy + p0.y*p0.y + p1.y*p1.y + p2.y*p2.y);
        // C.m22 += a * (9*pcz*pcz + p0.z*p0.z + p1.z*p1.z + p2.z*p2.z);
        // C.m01 += a * (9*pcx*pcy + p0.x*p0.y + p1.x*p1.y + p2.x*p2.y);
        // C.m02 += a * (9*pcx*pcz + p0.x*p0.z + p1.x*p1.z + p2.x*p2.z);
        // C.m12 += a * (9*pcy*pcz + p0.y*p0.z + p1.y*p1.z + p2.y*p2.z);
        p1 = p2;
        he = he.next;
        p2 = he.head.pnt;
    } while (he != he0);
    C.scale(1 / (12.0));
    // C is symmetric, so set symmetric components
    C.m10 = C.m01;
    C.m20 = C.m02;
    C.m21 = C.m12;
    return area;
}

Example 40 with Point3d

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

the class Face method computeCoords.

/**
 * Computes the barycentric coordinates of a point to the plane
 * @param pnt the point to consider
 * @param coords the returned coordinates
 */
public void computeCoords(Point3d pnt, Vector2d coords) {
    Vector3d v0 = new Vector3d();
    Vector3d v1 = new Vector3d();
    Vector3d v2 = new Vector3d();
    Point3d p0, p1, p2;
    HalfEdge he = he0;
    p0 = he.head.pnt;
    he = he.next;
    p1 = he.head.pnt;
    he = he.next;
    p2 = he.head.pnt;
    v0.sub(p1, p0);
    v1.sub(p2, p0);
    v2.sub(pnt, p0);
    double d00 = v0.dot(v0);
    double d01 = v0.dot(v1);
    double d11 = v1.dot(v1);
    double d20 = v2.dot(v0);
    double d21 = v2.dot(v1);
    double denom = d00 * d11 - d01 * d01;
    coords.x = (d11 * d20 - d01 * d21) / denom;
    coords.y = (d00 * d21 - d01 * d20) / denom;
}