Examples with Point artisynth.core.mechmodels.Point used on opensource projects

Example 1 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class FemModel3d method findContainingElement.

/**
 * Returns the element within an FEM that contains a specified
 * point, or <code>null</code> if there is no such element.
 *
 * @param pnt Point for which containing element is desired.
 * @return containing element, or null.
 */
public FemElement3d findContainingElement(Point3d pnt) {
    BVTree bvtree = getBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>(16);
    bvtree.intersectPoint(nodes, pnt);
    // System.out.println ("num nodes " + nodes.size());
    if (nodes.size() == 0) {
        return null;
    }
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            if (((FemElement3d) elements[i]).isInside(pnt)) {
                return (FemElement3d) elements[i];
            }
        }
    }
    return null;
}

Example 2 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class FemModel3d method findNearestNode.

/**
 * Finds the nearest node to a specified point that is within
 * a specified maximum distance. If no node is within the
 * specified maximum distance, <code>null</code> is returned.
 *
 * @param pnt Point for which the nearest node should be located
 * @param maxDist Maximum distance that the node must be from the
 * point. If <code>maxDist</code> &lt; 0, then <code>null</code>
 * will be returned.
 * @return Nearest point within the prescribed distance, or <code>null</code>
 * if there is no such point
 */
public FemNode3d findNearestNode(Point3d pnt, double maxDist) {
    if (maxDist < 0) {
        return null;
    }
    BVTree bvtree = getBVTree();
    ArrayList<BVNode> nodes = new ArrayList<BVNode>();
    bvtree.intersectSphere(nodes, pnt, maxDist);
    FemNode3d nearest = null;
    double dist = 1 + 2 * maxDist;
    for (BVNode n : nodes) {
        Boundable[] elements = n.getElements();
        for (int i = 0; i < elements.length; i++) {
            FemElement3d e = (FemElement3d) elements[i];
            for (int k = 0; k < e.numNodes(); k++) {
                double d = e.myNodes[k].getPosition().distance(pnt);
                if (d < dist && d <= maxDist) {
                    dist = d;
                    nearest = e.myNodes[k];
                }
            }
        }
    }
    return nearest;
}

Example 3 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class PointFem3dAttachment method computeVelDerivative.

private boolean computeVelDerivative(Vector3d dvel) {
    Frame pframe = myPoint.getPointFrame();
    boolean isNonZero = false;
    // can we optimize this? If the attachment has been enforced, then can we
    // compute lp2 and lv2 directly from the point itself?
    Vector3d lv2 = new Vector3d();
    Vector3d lp2 = new Vector3d();
    double[] coords = myCoords.getBuffer();
    for (int i = 0; i < myNodes.length; i++) {
        FemNode node = myNodes[i];
        double w = coords[i];
        lv2.scaledAdd(w, node.getLocalVelocity());
        lp2.scaledAdd(w, node.getLocalPosition());
    }
    if (myFemFrame != null) {
        RotationMatrix3d R2 = myFemFrame.getPose().R;
        Twist vel2 = myFemFrame.getVelocity();
        Vector3d tmp1 = new Vector3d();
        Vector3d tmp2 = new Vector3d();
        // R2*lp2
        tmp1.transform(R2, lp2);
        // R2*lv2
        tmp2.transform(R2, lv2);
        // tmp1 = w2 X R2*lp2 + R2*lv2
        tmp1.crossAdd(vel2.w, tmp1, tmp2);
        // dvel = w2 X R2*lv2 + w2 X tmp1
        dvel.cross(vel2.w, tmp2);
        dvel.crossAdd(vel2.w, tmp1, dvel);
        if (pframe != null) {
            RotationMatrix3d R1 = pframe.getPose().R;
            Twist vel1 = pframe.getVelocity();
            // R1*lv1
            tmp2.transform(R1, myPoint.getLocalVelocity());
            tmp2.negate();
            // tmp2 = -R1*lv1 - u2 + u1 - tmp1
            tmp2.sub(vel2.v);
            tmp2.add(vel1.v);
            tmp2.sub(tmp1);
            // dvel = R1^T (w1 X tmp2 + dvel)
            dvel.crossAdd(vel1.w, tmp2, dvel);
            dvel.inverseTransform(R1);
        }
        isNonZero = true;
    } else if (pframe != null) {
        RotationMatrix3d R1 = pframe.getPose().R;
        Twist vel1 = pframe.getVelocity();
        // dvel = R1^T (w1 X (u1 - R1*lv1 - lv2))
        // R1*lv1
        dvel.transform(R1, myPoint.getLocalVelocity());
        dvel.negate();
        // since Fem has no frame, lv2 and world velocity are the same
        dvel.sub(lv2);
        dvel.add(vel1.v);
        dvel.cross(vel1.w, dvel);
        dvel.inverseTransform(R1);
        isNonZero = true;
    }
    return isNonZero;
}

Example 4 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class SkinMeshBody method addAttachment.

// XXX
/**
 * Adds an attachment to this SkinMeshBody. The associated vertex is assumed
 * from the index of the new attachment point. If <code>initBase</code> is
 * true, then the attachment's base position is set to the current vertex
 * position. The number of attachments cannot exceed the current number of
 * mesh vertices.
 */
public void addAttachment(PointSkinAttachment a, boolean initBase) {
    if (numAttachments() >= numVertices()) {
        throw new IllegalArgumentException("Number of attachments may not exceed number of vertices");
    }
    if (a == null) {
        throw new IllegalArgumentException("Attachment is null");
    }
    int vidx = myVertexAttachments.size();
    myVertexAttachments.addNumbered(a, vidx);
    a.setSkinMesh(this);
    if (initBase) {
        a.setBasePosition(getVertex(vidx).getPosition());
    }
}

Example 5 with Point

use of artisynth.core.mechmodels.Point in project artisynth_core by artisynth.

the class SkinMeshBody method createPointAttachment.

public PointSkinAttachment createPointAttachment(Point pnt) {
    if (!(getMesh() instanceof PolygonalMesh)) {
        return null;
    }
    PolygonalMesh mesh = (PolygonalMesh) getMesh();
    if (!mesh.isTriangular()) {
        return null;
    }
    // Find nearest face to the point; we'll need this to
    // estimate a basePosition for the attachments from the
    // start by find
    BVFeatureQuery query = new BVFeatureQuery();
    Point3d near = new Point3d();
    Vector2d uv = new Vector2d();
    Face face = query.nearestFaceToPoint(near, uv, mesh, pnt.getPosition());
    // Create a new PointSkinAttachment
    MeshDistCalc dcalc = new MeshDistCalc();
    dcalc.computeDistancesAndWeights(pnt.getPosition(), myLastSigma);
    PointSkinAttachment a = dcalc.computeDisplacementAttachment();
    a.setSkinMesh(this);
    // Now estimate the basePosition from the face vertices
    Point3d basePos = new Point3d();
    Vertex3d[] vtxs = face.getTriVertices();
    double[] wgts = new double[] { 1 - uv.x - uv.y, uv.x, uv.y };
    for (int i = 0; i < vtxs.length; i++) {
        PointSkinAttachment va = (PointSkinAttachment) myVertexAttachments.get(vtxs[i].getIndex());
        basePos.scaledAdd(wgts[i], va.getBasePosition());
    }
    a.setBasePosition(basePos);
    a.setPoint(pnt);
    return a;
}