Examples with LineSegment maspack.geometry.LineSegment used on opensource projects

Example 1 with LineSegment

use of maspack.geometry.LineSegment in project artisynth_core by artisynth.

the class FemMuscleModel method getSegmentInsideSphere.

// intersects segment with the given sphere, and returns
// the portion inside or null if no intersection
private static LineSegment getSegmentInsideSphere(LineSegment segment, Point3d pos, double rad) {
    Point3d p1 = segment.myVtx0.getPosition();
    Point3d p2 = segment.myVtx1.getPosition();
    // check if segment starts inside
    boolean isP1Inside = isInsideSphere(p1, pos, rad);
    // p2-c
    Point3d p = new Point3d();
    // p1-p2
    Point3d dp = new Point3d();
    double r2 = rad * rad;
    // for use with quadratic equation
    double a, b, c, d, lambda1, lambda2;
    p.sub(p2, pos);
    dp.sub(p1, p2);
    // find intersection with sphere
    a = dp.normSquared();
    b = 2 * dp.dot(p);
    c = p.normSquared() - r2;
    d = b * b - 4 * a * c;
    if (d >= 0) {
        d = Math.sqrt(d);
        lambda1 = (-b + d) / (2 * a);
        lambda2 = (-b - d) / (2 * a);
    } else {
        lambda1 = Double.NaN;
        lambda2 = Double.NaN;
    }
    // if p2 is inside, return
    if (p.normSquared() <= r2) {
        if (isP1Inside) {
            return segment;
        } else {
            // find intersection
            if (lambda1 >= 0 && lambda1 <= 1) {
                p1.scaledAdd(lambda1, dp, p2);
            } else {
                p1.scaledAdd(lambda2, dp, p2);
            }
            return new LineSegment(new Vertex3d(p1), new Vertex3d(p2));
        }
    } else {
        // if p1 inside, find single intersection
        if (isP1Inside) {
            if (lambda1 >= 0 && lambda1 <= 1) {
                p2.scaledAdd(lambda1, dp);
            } else {
                p2.scaledAdd(lambda2, dp);
            }
            return new LineSegment(new Vertex3d(p1), new Vertex3d(p2));
        } else {
            // check if passes entirely through sphere
            if (d >= 0) {
                if (lambda1 >= 0 && lambda1 <= 1 && lambda2 >= 0 && lambda2 <= 1) {
                    p1.scaledAdd(lambda1, dp, p2);
                    p2.scaledAdd(lambda2, dp);
                    return new LineSegment(new Vertex3d(p1), new Vertex3d(p2));
                }
            }
        // done checking if crossed sphere
        }
    // done checking if p1 outside
    }
    return null;
}

Example 2 with LineSegment

use of maspack.geometry.LineSegment in project artisynth_core by artisynth.

the class FemMuscleModel method computeAverageFiberDirection.

/**
 * Computes the average fiber direction in the vicinity of a point based on
 * the line segments contained in a PolylineMesh. Returns the number of
 * supporting line segments used for the calculation. If no segments were
 * found, the method returns 0 and the direction is undefined.
 *
 * @param dir returns the normalized direction
 * @param pos position at which direction should be computed
 * @param rad radius of influence within which polyline mesh segments are
 * considerd
 * @param mesh mesh containing line segments used to determine the direction
 * @return number of supporting line segment
 */
public static int computeAverageFiberDirection(Vector3d dir, Point3d pos, double rad, PolylineMesh mesh) {
    BVTree bvh = mesh.getBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    Matrix3d cov = new Matrix3d();
    SVDecomposition3d svd = new SVDecomposition3d();
    Vector3d tmp = new Vector3d();
    Matrix3d tmp2 = new Matrix3d();
    bvh.intersectSphere(nodes, pos, rad);
    dir.setZero();
    int nsegs = 0;
    // for computing sign of direction vector
    Vector3d segmentSum = new Vector3d();
    // System.out.println("p=[");
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            LineSegment seg = (LineSegment) elements[i];
            seg = getSegmentInsideSphere(seg, pos, rad);
            if (seg != null) {
                tmp.sub(seg.myVtx1.pnt, seg.myVtx0.pnt);
                if (tmp.norm() >= 1e-8 * rad) {
                    // System.out.println(seg.myVtx0.getPosition() + " " +
                    // seg.myVtx1.getPosition());
                    nsegs++;
                    // prepare to average directions using SVD
                    computeCov(tmp2, tmp);
                    cov.add(tmp2);
                    segmentSum.add(tmp);
                }
            }
        }
    }
    if (nsegs > 0) {
        // we are technically including both +/- directions, so
        // we have twice the number of points
        cov.scale(2.0 / (2.0 * nsegs - 1));
        try {
            svd.factor(cov);
        } catch (Exception e) {
        // System.err.println(e.getMessage());
        }
        // principal components
        tmp2 = svd.getU();
        tmp2.getColumn(0, dir);
        dir.normalize();
        // most line segments
        if (dir.dot(segmentSum) < 0) {
            dir.scale(-1);
        }
        return nsegs;
    } else {
        return 0;
    }
}

Example 3 with LineSegment

use of maspack.geometry.LineSegment in project artisynth_core by artisynth.

the class MFreeMuscleModel method getSegmentInsideSphere.

// intersects segment with the given sphere, and returns
// the portion inside or null if no intersection
private static LineSegment getSegmentInsideSphere(LineSegment segment, Point3d pos, double rad) {
    Point3d p1 = segment.myVtx0.getPosition();
    Point3d p2 = segment.myVtx1.getPosition();
    // check if segment starts inside
    boolean isP1Inside = isInsideSphere(p1, pos, rad);
    // p2-c
    Point3d p = new Point3d();
    // p1-p2
    Point3d dp = new Point3d();
    double r2 = rad * rad;
    // for use with quadratic equation
    double a, b, c, d, lambda1, lambda2;
    p.sub(p2, pos);
    dp.sub(p1, p2);
    // find intersection with sphere
    a = dp.normSquared();
    b = 2 * dp.dot(p);
    c = p.normSquared() - r2;
    d = b * b - 4 * a * c;
    if (d >= 0) {
        d = Math.sqrt(d);
        lambda1 = (-b + d) / (2 * a);
        lambda2 = (-b - d) / (2 * a);
    } else {
        lambda1 = Double.NaN;
        lambda2 = Double.NaN;
    }
    // if p2 is inside, return
    if (p.normSquared() <= r2) {
        if (isP1Inside) {
            return segment;
        } else {
            // find intersection
            if (lambda1 >= 0 && lambda1 <= 1) {
                p1.scaledAdd(lambda1, dp, p2);
            } else {
                p1.scaledAdd(lambda2, dp, p2);
            }
            return new LineSegment(new Vertex3d(p1), new Vertex3d(p2));
        }
    } else {
        // if p1 inside, find single intersection
        if (isP1Inside) {
            if (lambda1 >= 0 && lambda1 <= 1) {
                p2.scaledAdd(lambda1, dp);
            } else {
                p2.scaledAdd(lambda2, dp);
            }
            return new LineSegment(new Vertex3d(p1), new Vertex3d(p2));
        } else {
            // check if passes entirely through sphere
            if (d >= 0) {
                if (lambda1 >= 0 && lambda1 <= 1 && lambda2 >= 0 && lambda2 <= 1) {
                    p1.scaledAdd(lambda1, dp, p2);
                    p2.scaledAdd(lambda2, dp);
                    return new LineSegment(new Vertex3d(p1), new Vertex3d(p2));
                }
            }
        // done checking if crossed sphere
        }
    // done checking if p1 outside
    }
    return null;
}

Example 4 with LineSegment

use of maspack.geometry.LineSegment in project artisynth_core by artisynth.

the class MFreeMuscleModel method computeAverageFiberDirection.

public static boolean computeAverageFiberDirection(Vector3d dir, Point3d pos, double rad, PolylineMesh mesh) {
    BVTree bvh = mesh.getBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    Matrix3d cov = new Matrix3d();
    SVDecomposition3d svd = new SVDecomposition3d();
    Vector3d tmp = new Vector3d();
    Matrix3d tmp2 = new Matrix3d();
    bvh.intersectSphere(nodes, pos, rad);
    dir.setZero();
    int nsegs = 0;
    // for computing sign of direction vector
    Vector3d segmentSum = new Vector3d();
    // System.out.println("p=[");
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            LineSegment seg = (LineSegment) elements[i];
            seg = getSegmentInsideSphere(seg, pos, rad);
            if (seg != null) {
                tmp.sub(seg.myVtx1.pnt, seg.myVtx0.pnt);
                if (tmp.norm() >= 1e-8 * rad) {
                    // System.out.println(seg.myVtx0.getPosition() + " " +
                    // seg.myVtx1.getPosition());
                    nsegs++;
                    // prepare to average directions using SVD
                    computeCov(tmp2, tmp);
                    cov.add(tmp2);
                    segmentSum.add(tmp);
                }
            }
        }
    }
    if (nsegs > 0) {
        // we are technically including both +/- directions, so
        // we have twice the number of points
        cov.scale(2.0 / (2.0 * nsegs - 1));
        try {
            svd.factor(cov);
        } catch (Exception e) {
        // System.err.println(e.getMessage());
        }
        // principal components
        tmp2 = svd.getU();
        tmp2.getColumn(0, dir);
        dir.normalize();
        // most line segments
        if (dir.dot(segmentSum) < 0) {
            dir.scale(-1);
        }
        return true;
    } else {
        return false;
    }
}