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

Example 11 with Point

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

the class MotionTargetTerm method doAddTarget.

/**
 * Adds a target to the term for trajectory error
 * @param source
 * @param weight
 * @return the created target body or point
 */
private MotionTargetComponent doAddTarget(MotionTargetComponent source, double weight) {
    mySources.add(source);
    // myController.sourcePoints.add (source);
    source.setTargetActivity(TargetActivity.None);
    MotionTargetComponent target = null;
    if (source instanceof Point) {
        myTargetVelSize += POINT_VEL_SIZE;
        myTargetPosSize += POINT_POS_SIZE;
        target = addTargetPoint((Point) source);
    } else if (source instanceof Frame) {
        myTargetVelSize += FRAME_VEL_SIZE;
        myTargetPosSize += FRAME_POS_SIZE;
        target = addTargetFrame((RigidBody) source);
    }
    myTargetWeights.add(weight);
    updateWeightsVector();
    // set target matrix null, so that it is recreated on demand
    // XXX should be updated on a change event...
    myVelJacobian = null;
    // mySolver.resetVariables();
    return target;
}

Example 12 with Point

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

the class MotionTargetTerm method interpolateTargetVelocity.

/*
    * calculate target velocity by differencing current and last target positions
    */
private void interpolateTargetVelocity(double h) {
    // paranoid
    assert myTargets.size() == mySources.size();
    updateTargetPos(h);
    if (prevTargetPos == null || prevTargetPos.size() != myTargetPosSize) {
        prevTargetPos = new VectorNd(myTargetPos);
    }
    double[] prevPosBuf = prevTargetPos.getBuffer();
    int idx = 0;
    for (int i = 0; i < myTargets.size(); i++) {
        MotionTargetComponent target = myTargets.get(i);
        if (target instanceof Point) {
            idx = tmpPoint.setPosState(prevPosBuf, idx);
            interpolateTargetVelocityFromPositions(tmpPoint, (Point) target, h);
        } else if (target instanceof Frame) {
            idx = tmpFrame.setPosState(prevPosBuf, idx);
            interpolateTargetVelocityFromPositions(tmpFrame, (Frame) target, h);
        }
    }
    prevTargetPos.set(myTargetPos);
}

Example 13 with Point

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

the class MotionTargetTerm method removeTarget.

/**
 * Removes a target to the term for trajectory error
 * @param source
 */
protected void removeTarget(MotionTargetComponent source) {
    int idx = mySources.indexOf(source);
    if (idx == -1) {
        return;
    }
    if (source instanceof Point) {
        myTargetVelSize -= POINT_VEL_SIZE;
        myTargetPosSize -= POINT_POS_SIZE;
        // remove ref particle created in doAddTarget
        removeTargetPoint((Point) myTargets.get(idx));
    } else if (source instanceof Frame) {
        myTargetVelSize -= FRAME_VEL_SIZE;
        myTargetPosSize -= FRAME_POS_SIZE;
        // remove ref body created in doAddTarget
        removeTargetFrame((Frame) myTargets.get(idx));
    }
    myTargetWeights.remove(idx);
    mySources.remove(idx);
    myTargets.remove(idx);
    updateWeightsVector();
    // set target matrix null, so that it is recreated on demand
    // XXX should be updated on a change event...
    myVelJacobian = null;
// Main.getMain().getInverseManager().configureTargetProbes();
// mySolver.resetVariables();
}

Example 14 with Point

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

the class MFreeMeshComp method createEmbedded.

/**
 * Assumes the mesh is in "rest" coordinates, not current coordinates
 * @param surf mfree surface to populate (or null to create one)
 * @param mesh mesh to embed
 * @param mfree model to embed mesh in
 * @return populated or created mesh
 */
public static MFreeMeshComp createEmbedded(MFreeMeshComp surf, MeshBase mesh, MFreeModel3d mfree) {
    double reduceTol = 1e-8;
    ArrayList<MFreeNode3d> nodes = new ArrayList<MFreeNode3d>();
    VectorNd weights = new VectorNd();
    if (surf == null) {
        surf = new MFreeMeshComp(mfree);
    }
    surf.setMesh(mesh);
    ArrayList<Vertex3d> verts = mesh.getVertices();
    Point3d coords = new Point3d();
    ArrayList<FemNode3d> deps = new ArrayList<>();
    MLSShapeFunction sfunc = new MLSShapeFunction();
    surf.myVertexAttachments.clear();
    for (int i = 0; i < verts.size(); i++) {
        // this could works very similarly to the code that adds
        // marker points into a mesh
        Vertex3d vtx = verts.get(i);
        deps.clear();
        mfree.findDependentNodesAtRest(vtx.pnt, deps);
        // compute shape function
        VectorNd N = new VectorNd(deps.size());
        MFreeNode3d[] dnodes = deps.toArray(new MFreeNode3d[deps.size()]);
        sfunc.update(vtx.pnt, dnodes);
        sfunc.eval(N);
        // first see if there's a node within reduceTol of the point,
        // and if so just use that
        double maxDist = Double.NEGATIVE_INFINITY;
        double minDist = Double.POSITIVE_INFINITY;
        MFreeNode3d nearestNode = null;
        for (int k = 0; k < dnodes.length; k++) {
            double d = vtx.pnt.distance(dnodes[k].getRestPosition());
            if (d > maxDist) {
                maxDist = d;
            }
            if (d < minDist) {
                minDist = d;
                nearestNode = dnodes[k];
            }
        }
        if (minDist / maxDist <= reduceTol) {
            // weight everything to the nearest node
            nodes.clear();
            nodes.add(nearestNode);
            weights.setSize(0);
            weights.append(1.0);
        } else {
            nodes.clear();
            weights.setSize(0);
            for (int k = 0; k < N.size(); k++) {
                if (Math.abs(N.get(k)) >= reduceTol) {
                    nodes.add(dnodes[k]);
                    weights.append(N.get(k));
                }
            }
        }
        if (weights.size() > 1) {
            PointFem3dAttachment attacher = new PointFem3dAttachment();
            attacher.setFromNodes(nodes, weights);
            surf.myVertexAttachments.add(attacher);
        } else if (weights.size() == 1) {
            PointParticleAttachment attacher = new PointParticleAttachment(nodes.get(0), null);
            surf.myVertexAttachments.add(attacher);
        }
    }
    surf.buildNodeVertexMap();
    return surf;
}

Example 15 with Point

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

the class MFreeModel3d 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 = getElementBVTree();
    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.getNodes()[k].getPosition().distance(pnt);
                if (d < dist && d <= maxDist) {
                    dist = d;
                    nearest = e.getNodes()[k];
                }
            }
        }
    }
    return nearest;
}